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ABSTRACT 


The  ability  of  an  autonomous  vehicle  control  system  to 
plan  a  safe,  collision-free  local  path  from  one  vehicle 
position  to  another  is  one  of  the  most  important 
functions.  In  this  thesis,  it  is  shown  how  a  safe 
obstacle-free  local  path  can  be  planned  using  optimal 
control  theory  and  optimization  techniques.  The  problem 
is  posed  as  a  two  point  boundary  value  problem  with 
various  problem  constraints  which  control  the  vehicle 
behavior  in  transversing  from  one  point  to  another.  The 
objective  function  being  minimized  is  a  control 
performance  index  which  includes  vehicle  energy  saving 
parameters.  Numerous  fixed  and  moving  obstacles  in  the 
dive  plane  are  introduced  and  successfully  avoided  using 
this  technique.  Three  dimensional  path  planning  is  also 
successfully  demonstrated  on  a  12  state  linear  model  of  an 


underwater  vehicle.  This  technique  is  shown  to  be  a 
feasible  method  for  local  path  planning  applications. 
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THESIS  DISCLAIMER 

The  reader  is  cautioned  that  computer  programs 
developed  in  this  research  may  not  have  been  exercised  for 
all  cases  of  interest.  While  every  effort  has  been  made, 
within  the  time  available,  to  ensure  that  the  programs  are 
free  of  computational  and  logic  errors,  they  can  not  be 
considered  validated.  Any  application  of  these  programs 
without  additional  verification  is  at  the  risk  of  the 
user. 
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I .  INTRODUCTION 


A.  GENERAL 

The  presently  forecast  missions  of  an  Autonomous 
Underwater  Vehicle  (AUV)  vary  in  scope  from  mine  detection 
and  avoidance  to  surveying  the  bottom  of  oceans.  Further, 
it  is  expected  that  many  of  these  missions  will  be 
conducted  within  the  context  of  military  objectives. 
Admiral  William  H.  Rowden,  Commander  Naval  Sea  Systems 
Command  stated  that,  "With  the  NAVSEA  (Naval  Sea  System 
Command)  Integrated  Robotics  Program  about  to  enter  its 
fifth  year  of  existence,  it  seems  appropriate  to  look  back 
and  ahead  to  establish  a  baseline  for  the  promulgation  of 
policy  guidelines  to  facilitate  the  continuing  evolution 
of  this  important  program."  [Ref.  1]  He  goes  on  to  say 
that  the  time  has  come  to  incorporate  the  value  of 
robotics  and  automation  into  the  Navy's  expanding  mission. 
Recent  articles  of  Military  Robotics  [Refs.  2-6],  have 
pointed  out  the  increased  availability  of  robotic 
vehicles.  These  include  Remotely  Piloted  Aircraft, 
Unmanned  Submarines,  Teleoperated  Combat  Vehicles,  Cruise 
Missiles  and  Teleoperated  and  Autonomous  Weapons. 

An  extremely  important  part  of  the  total  AUV  vehicle 
control  logic  is  its  need  to  plan  and  execute  a  safe 
passage  in  the  undersea  environment.  Local  path  planning 


is  the  function  provided  by  an  intelligent  system,  which 
determines  safe,  collision-free  trajectory  of  travel 
between  two  points,  a  start  point  and  a  target  point,  for  a 
specific  time  lapse.  One  possible  total  system  block 
diagram  that  shows  how  the  local  path  planner  could  be 
interfaced,  is  shown  in  Figure  1.1.  Here,  the  Global 
Planning  System  would  provide  the  Local  Path  Planner  with  a 
series  of  data  sets.  Included  in  the  data  sets  would  be 
destination  position,  destination  time,  start  position, 
start  time,  obstacles  and  boundaries.  In  return,  the  path 
planner  would  provide  an  optimal  path  based  upon  the 
limitations  of  the  vehicle  dynamics,  power  plant 
efficiency,  obstacle  field,  and  required  maneuver  time. 

Numerous  techniques  have  been  used  to  achieve  collision 
free  local  paths  for  various  vehicle  types  and 
manipulators.  These  include  graphical  search  methods  [Refs 
7-11],  potential  field  methods  [Refs.  12-16]  and  optimal 
control  theory  [Refs.  17,  18].  This  thesis  is  concerned 
with  developing  a  method  of  autonomous  planning  using 
optimal  control  theory. 

B.  PREVIOUS  WORK 

A  basic  investigation  of  local  path  planning  was 
previously  conducted  using  optimal  control  theory 
[Ref.  19].  In  that  study,  major  emphasis  was  placed  on  the 
solution  of  a  SISO  (Single  Input  Single  Output)  problem,  a 
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Figure  1.1  Global  Planning  System 


MIMO  (Multiple  Input  Multiple  Output)  problem  and  its 
generalization  to  a  submersible.  That  work  included 
objective  function  determination,  integration  method 
studies,  linear  versus  nonlinear  solution  results, 
computational  expense  and  an  obstacle  avoidance  solution 
with  one  fixed  obstacle.  The  objective  function  used  for 
optimization  was  a  quadratic  performance  index  of  the  form: 

J  =  r  FINTIM  (xTqx  +  utRU) dt 

J  0  ~  “ 


where , 


U  =  the  control  vector;  and 


X  =  desired  states-actual  states  (i.e.  state  error) 

The  nonlinear  hydrodynamic  equations  of  motion  for  the 
Autonomous  Underwater  Vehicle  being  studied  were  of  the 
following  form: 

•  •  • 

MX  +  f(X,  X)  =  g(U) 

The  "best"  solution  was  obtained  by  minimizing  the 
objective  function  (J)  in  order  to  find  the  best  U(t)  and 
X(t)  values. 

The  Automatic  Design  Synthesis  (ADS)  Fortran  Program 
[Ref.  20]  was  utilized  for  problem  optimization  and  the 
Dynamic  Simulation  Language  (DSL)  Program  [Ref.  21]  was 
utilized  for  objective  function  calculations  and 
integrations  of  the  vehicle  dynamic  equations.  These 
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software  programs  were  made  to  be  interactive  and  now 
perform  as  one  software  package  [Ref.  22].  The  combined 
package  is  called  ADSL  and  has  been  incorporated  on  the  IBM 
3033  Mainframe  Computer  System  at  the  Naval  Postgraduate 
School.  The  basic  optimization  approach  was  as  follows: 

1.  Discretize  the  control  vector  into  a  time-wise 

uniform  distribution  of  control  signals  (Figure  1.2) 


TIME  (Sec) 


Figure  1.2  Discrete  vs.  Continuous  Controls 

2.  Determine  the  best  control  sequence  via  an 
optimization  routine  based  upon  the  objective 
function  and  problem  constraints. 

In  the  two-dimensional  problem  (dive  plane  only) ,  the 

vehicle  was  ordered  to  achieve  an  ordered  depth  of  17.425 
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feet  using  minimum  bow  and  stem  plane  deflections. 
Additionally,  the  vehicle  was  required  to  have  a  minimum 
pitch  angle  at  its  final  end  condition.  The  control  vector 
(U)  was  the  bow  and  stem  plane  angles,  while  the  X  vector 
was  the  x  and  z  positions  of  the  vehicle  and  velocities  in 
the  x  and  z  directions. 

C.  AIM  OF  THE  PRESENT  STUDY 

This  thesis  is  concerned  with  furthering  the 
understanding  of  local  path  planning  using  optimal  control 
theory.  The  purpose  of  this  work  is  to: 

1.  Further  develop  the  planning  level  control  logic 
to  consider  three-dimensional  maneuvers,  and 

2.  Evaluate  the  performance  of  this  logic. 
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The  basic  approach  was  as  follows: 

1.  Improve  the  treatment  of  obstacles,  both  fixed  and 
moving  in  the  two-dimensional  problem. 

2.  Determine  the  best  set  of  optimization  program 
options  based  on  computational  cost,  robustness, 
flexibility  and  solution  accuracy  in  the  two-dimensional 
problem. 

3.  Select  guidance  for  maneuvering  time  (FINTIM)  and 
determine  how  it  effects  problem  solution  in  the  two- 
dimensional  problem. 

4.  Evaluate  two  dimensional  versus  three  dimensional 
computational  costs  and  accuracy. 

The  basic  assumption  in  this  study  was  that  the  work 
previously  done  [Ref.  19]  remained  valid.  Specifically, 
that  the  integration  method  selected,  the  step  size, 
objective  function,  number  of  design  variables  and 
optimization  program  options  remained  relevant. 


III.  OBSTACLE  AVOIDANCE 


The  approach  previously  presented  [Ref.  19]  was  to 
compute  the  distance  to  the  obstacle  at  ten  equally  divided 
time  intervals  from  start  time  to  the  time  of  closest 
obstacle  approach.  These  updated  distances  were  then 
incorporated  into  the  optimization  algorithm  for  constraint 
value  determination.  ADSL  placed  constraint  equations  into 
the  algorithm  in  the  form: 

Gj  (k)  =  0  j  =  1,  m 
which  in  the  actual  program  is: 

Gk(k)  =  (avoidance  zone)  -  (updated  vehicle  distance) 
The  time  interval  for  distance  calculations  was  determined 
based  on  the  FINTIM  and  clock  time  as  follows: 

If  (time.ge.O.O.and.le.xobs/u)  then 
timel  =  xobs/u 

qn  =  time/ (timel/10.  -  delt/10000.) 
d  =  int(qn  +  1) 

dist(d)  =  sqrt ( (xpos-xobs)  x  (zpos-zobs)) 
g(d)  -  l.  -  dist(d) 
where : 

time  =  DSL  clock  time 
xobs  =  x  position  of  the  obstacle 
delt  =  integration  time  step  interval 
xpos  =  x  position  of  the  vehicle 
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u  =  vehicle  velocity  in  the  x  direction 

dist(d)  -  computed  distance  from  the  vehicle  to  the 
obstacle 

zpos  =  z  position  of  the  vehicle 

zobs  =  z  position  of  the  obstacle 

g(d)  *  constraint  value  placed  in  optimization  routine 

The  problem  with  this  approach  is  that  the  further  an 
obstacle  is  from  the  start  position,  the  longer  the  time 
intervals  become  for  obstacle  distance  calculations.  This 
is  satisfactory  for  a  single  fixed  obstacle  but  for 
multiple  obstacles,  this  method  results  in  inadequate 
distance  computations.  This  is  because  the  closer 
obstacles  do  not  have  sufficient  constraint  inputs  compared 
to  the  distant  obstacles.  As  a  result,  the  distant 
obstacles  tend  to  dominate  the  solution.  Using  multiple 
"if”  statements  in  this  logic  is  also  computationally 
expensive  and  failed  when  used  with  three  or  more 
obstacles. 

Another  inherent  problem  was  that  distances  were  not 
computed  after  the  time  of  closest  approach.  This 
sometimes  resulted  in  maneuvers  with  distances  to  the 
obstacle  that  violated  the  avoidance  zone  regions  after  the 
first  time  of  closest  approach. 

A  better  method  is  to  continuously  compute  distances  to 
the  obstacle,  independently  of  FINTIM,  but  dependent  on 
FINTIM  step  intervals.  This  approach  worked  very  well  and 
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was  adopted  for  all  further  analysis.  An  additional 
advantage  to  this  approach  is  that  only  one  constraint 
assignment  was  needed  for  each  obstacle  vice  ten. 

The  computational  time  for  obstacle  avoidance  varies  as 
the  number  of  obstacles  increases.  The  motivation  for  this 
study  was  to  determine  if  this  approach  was  computationally 
too  expensive  to  remain  as  a  viable  approach.  Figure  3.1 
shows  the  computational  cost  for  one  to  seventeen  fixed 
obstacles.  The  computational  time  is  based  on  the  virtual 
machine  time  for  the  IBM  3033  system  at  the  Naval 
Postgraduate  School.  In  all  cases,  the  final  depth  was  the 
desired  depth  of  17.425  feet. 

Various  optimization  techniques  have  various 
computational  costs;  however,  the  times  in  Figure  3.1  are 
based  upon  the  final  optimization  option  selected  for  this 
thesis.  The  selection  criteria  with  results  will  be 
presented  in  the  following  chapter. 
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Figure  3.1  Fixed  Obstacle  Computational  Results 


IV 


The  ADS  (Advanced  Design  Synthesis)  Program  allows  for 
the  selection  of  numerous  optimization  techniques  for 
problem  solution.  There  are  three  levels  by  which  to 
select  a  particular  technique.  The  three  levels  are  the 
strategy  level,  optimizer  level  and  the  one-dimensional 
search  level.  Table  1  lists  the  various  levels  and  the 
various  methods  contained  in  each.  Figure  4.1  identifies 
the  large  number  of  possible  algorithm  combinations 
allowed.  Vanderplaats  provides  a  detailed  discussion  of 
the  various  methods  and  algorithms  in  Reference  23. 

A.  CLUSTERED  FIXED  OBSTACLE  TEST 

In  order  to  be  effective  as  a  path  planning  algorithm, 
it  is  necessary  for  the  program  option  to  be  robust  and 
flexible  enough  to  solve  problems  involving  numerous  fixed 
obstacles  as  well  as  moving  obstacles.  Therefore,  an 
initial  test  was  conducted  where  the  number  of  obstacles  in 
the  vehicle's  path  were  varied  from  one  to  seventeen. 
Program  option  057  was  selected  first  based  upon  the 
recommendation  of  the  previous  study,  which  involved  one 
fixed  obstacle  in  the  vehicle's  path.  Option  057  appeared 
acceptable  until  a  four  obstacle  field  was  encountered.  At 
this  point,  the  option  failed  to  reach  an  adequate 
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IOPT 


OPTIMIZER 


1  Fletcher-Reeves 

2  Davidon-Fletcher-Powell  (DFP) 

3  Broydon-Fletcher-Goldfarb-Shanno  (BFGS) 

4  Method  of  Feasible  Directions 

5  Modified  Method  of  Feasible  Directions 


STRATEGY 


I8TRAT  IOPT  12345 


None  0 
SUMT,  Exterior  1 
SUMT,  Linear  Extended  Interior  2 
SUMT,  Quadratic  Extenden  Interior  3 
SUMT,  Cubic  Extended  Interior  4 
Augmented  Lagrange  Multiplier  Meth.  5 
Sequential  Linear  Programming  6 
Method  of  Centers  7 
Sequential  Quadratic  Programming  8 
Sequential  Convex  Programming  9 


X  X  X  X  X 
X  X  X  0  0 
X  X  X  0  0 
X  X  X  0  0 
X  X  X  0  0 
X  X  X  0  0 
0  0  0  X  X 
0  0  0  X  X 
0  0  0  X  X 
0  0  0  X  X 


Qtg-DIMENSiggAL  SEARCH _ IONgp 

Golden  Section  Method  1 
Golden  Section  +  Polynomial  2 
Polynomial  Interpolation  (bounded)  3 
Polynomial  Extrapolation  4 
Golden  Section  Method  5 
Golden  Section  +  Polynomial  6 
Polynomial  Interpolation  (bounded)  7 
Polynomial  Extrapolation  8 


X  X  X  0  0 
X  X  X  0  0 
X  X  X  0  0 
X  X  X  0  0 
0  0  0  X  X 
0  0  0  X  X 
0  0  0  X  X 
0  0  0  X  X 


NOTE:  An  X  denotes  an  allowed  combination  of  algorithms. 


Figure  4.1  Allowable  ADS  Algorithm  Combinations 


1  3 


TABLE  1.  ADS  LEVEL  OPTIONS 


STRATEGY  (ISRRAT) 

0  -  None 

1  -  SUMT,  Exterior  Penalty  Function 

2  -  SUMT,  Linear  Extended  Interior 

3  -  SUMT,  Quadratic  Extended  Interior 

4  -  Cubic  Extended  Interior 

5  -  Augmented  Lagrange  Multiplier  Method 

6  -  Sequential  Linear  Programming 

7  -  Method  of  Centers 

8  -  Sequential  Quadratic  Programming 

9  -  Sequential  Convex  Programming 
OPTIMIZER  (IOPT) 

1  -  Fletcher-Reeves 

2  -  Davidon-Fletcher-Powell  (DFP) 

3  -  Broydon-Fletcher-Golfarb-Shanno  (BFGS) 

4  -  Method  of  Feasible  Directions 

5  -  Modified  Method  of  Feasible  Directions 
ONE-DIMENSIONAL  SEARCH  (IONED) 

1  -  Golden  Section  Method 

2  -  Golden  Section  and  Polynomial 

3  -  Polynomial  Interpolation,  bounded 

4  -  Polynomial  Extrapolation 
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5  -  Golden  Section  Method 

6  -  Golden  Section  and  Polynomial 

7  -  Polynomial  Interpolation,  bounded 

8  -  Polynomial  Extrapolation 


1  5 


solution.  Option  133  was  then  selected  based  upon  Olson's 
work  [Ref.  22].  Option  133  is  more  robust  than  option  057 
and  it  appeared  to  be  very  well  suited  for  this  problem 
until  the  ten  obstacle  field  was  encountered.  This  method 
achieved  the  correct  ordered  depth;  however,  it  violated 
many  of  the  obstacle  avoidance  zones.  It  was  apparent,  at 
this  point,  that  all  program  options  would  have  to  be 
tested  in  order  to  determine  the  most  appropriate 
algorithm. 

A  test  was  conducted  to  determine  if  any  of  the  one 
hundred  and  twelve  program  options  could  solve  a  seventeen 
obstacle  problem.  The  complete  test  problem  required  the 
program  option  to  solve  a  seventeen  obstacle  field  problem 
with  FINTIM  set  at  seven  seconds  and  an  ordered  depth  of 
17.425  feet.  Each  obstacle  had  a  one  foot  radius  avoidance 
zone.  The  results  of  that  study  are  presented  in  Table  2. 


TABLE  2.  17  OBSTACLE  TEST  RESULT 


PROGRAM  OPTION 

COMPUTATIONAL  COST 

DEPTH 

311 

(sec) 

(feet) 

87.92 

17.425 

312 

92.56 

17.425 

313 

58.78 

17.425 

314 

73.51 

17.425 

321 

146.23 

17.425 

322 

142.32 

17.425 

323 

115.11 

17.425 

324 

165.12 

17.425 

331 

167.25 

17.425 

332 

136.84 

17.425 

333 

151.90 

17.425 

334 

120.78 

17.425 

411 

76.19 

17.425 

412 

96.00 

17.421 

413 

42.99 

17.425 

414 

64.92 

17.424 

421 

116.61 

17.425 

422 

140.87 

17.425 

423 

82.18 

17.425 

424 

94.39 

17.425 

431 

115.67 

17.425 

432 

143.50 

17.425 

433 

81.64 

17.425 

434 

97.29 

17.425 

512 

70.17 

17.425 

534 

48.87 

17.449 

Of  the  one  hundred  and  twelve  program  options,  only  twenty 
six  achieved  a  correct  solution.  The  computational  time 
varied  significantly  among  the  various  successful  program 
options.  In  some  cases,  the  exact  depth  was  not  achieved; 
however,  all  results  were  considered  excellent. 

After  determining  which  algorithms  could  solve  the 
seventeen  obstacle  problem,  it  was  then  necessary  to  verify 
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that  cases  involving  various  obstacle  combinations  from  one 
to  sixteen  were  solvable  by  these  methods.  It  may  seem 
intuitively  obvious  that  if  an  algorithm  can  achieve  a 
solution  involving  seventeen  obstacles  that  it  can  solve 
all  other  cases  from  one  obstacle  to  sixteen  obstacles. 
Contrary  to  intuition,  this  is  not  the  case.  For  example, 
program  option  313,  which  had  a  relatively  small 
computational  cost,  successfully  solved  the  seventeen 
obstacle  problem  but  failed  to  achieve  the  correct  depth 
when  an  eleven  obstacle  field  was  encountered. 

In  conducting  the  varying  fixed  obstacle  investigation, 
obstacles  were  purposefully  placed  in  various  positions  in 
the  field  in  order  to  ensure  that  obstacle  position  had  no 
negative  effect  upon  the  problem  solution.  This  was 
significant  because  program  option  057  (method  chosen  in 
the  previous  study) ,  failed  when  it  encountered  an  obstacle 
field  with  three  fixed  obstacles.  Two  obstacles  were 
placed  in  the  vehicle's  path  and  one  was  placed  far  from 
the  vehicle's  path.  The  obstacle  far  away  from  the 
vehicle's  path  was  determined  to  be  the  cause  of  failure 
because  the  algorithm  successfully  solved  a  problem  with 
three  obstacles  when  all  three  were  placed  near  the 
vehicle's  path.  Using  the  cases  of  one  to  seventeen 
obstacles,  the  cases  were  further  reduced  from  26  to  22. 
Program  options  313,  413,  534,  and  314  were  eliminated. 


B.  IMPOSSIBLE  FIELD  TEST 

After  an  investigation  of  the  varying  obstacle  test, 
the  reduced  list  of  programs  were  subjected  to  an 
impossible  problem.  Four  obstacles  were  placed  in  the 
vehicle's  path  with  nine,  five,  three,  and  six  feet  radii. 
They  were  placed  in  such  a  way  that  the  algorithm  could  not 
achieve  the  correct  solution  in  the  allotted  time.  It  is 
important  to  point  out  that  a  correct  solution  would  have 
been  obtainable  if  the  simulation  time  was  increased.  The 
motivation  for  this  study  was  to  determine  the  failure 
modes  of  various  algorithms.  It  was  evident  from  this 
study  that  some  algorithms,  namely  those  which  employed  a 
strategy  of  Sequential  Unconstrained  Minimization  using  the 
Cubic  Extended  Interior  Penalty  Function  Method,  were  more 
sensitive  to  achieving  the  desired  depth  constraints,  when 
they  were  imposed  as  equality  constraints.  The  algorithms 
which  employed  the  strategy  of  Sequential  Unconstrained 
Minimization  using  the  Quadratic  Extended  Interior  Penalty 
Function  Method,  were  more  sensitive  in  avoiding  obstacle 
avoidance  zones  when  they  were  imposed  as  inequality 
constraints.  Figure  4.2  illustrates  the  performance  of 
three  different  algorithms  in  solving  this  problem.  Of  the 
algorithms  with  relatively  small  computational  costs, 
program  option  311  did  the  best  job  of  avoiding  the 
avoidance  zones. 
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Figure  4.2  Impossible  Test  Algorithm  Comparison 


With  the  impossible  field  test,  computational  cost 
uniformly  increased  compared  to  the  four  obstacle  problem 
with  smaller  avoidance  zones.  Program  option  311  had  a 
computational  cost  of  74.30  Virtual  Machine  second  in  the 
impossible  problem;  however,  with  four  obstacles  it  took  21 
seconds  (Figure  3.1).  Figure  4.3  is  the  solution  result 
obtained  if  FINTIM  is  increased  to  15.0.  Although  FINTIM 
was  more  than  doubled,  the  computational  cost  did  not 
significantly  increase.  With  FINTIM  set  to  15.0,  the 
virtual  machine  time  was  76.51  seconds. 

C.  SELECTION  RESULT 

Program  option  311  with  a  strategy  of  Sequential 
Unconstrained  Minimization  using  the  Quadratic  Extended 
Interior  Penalty  Function  Method;  an  Optimizer  using  the 
Fletcher-Reeves  algorithm  and  a  one-dimensional  search 
method  using  the  Golden  Section  Method  was  chosen  as  the 
best  algorithm.  It  was  selected  because  it  had  the  least 
computational  cost  of  any  algorithm  which  could  solve  the 
seventeen  obstacle  test  problem  and  was  very  sensitive  to 
the  obstacle  avoidance  zones.  In  other  words,  it  proved  to 
be  very  good  at  finding  a  safe,  collision-free  path  between 
the  start  condition  and  the  end  condition. 


(li)  Hld3Q 
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Figure  4.3  Solution  of  Impossible  Test  with  Increased  FINTIM 


V.  EVALUATION  07  MANEUVERING  TIKE  (FINTIM) 


Qualitatively,  there  are  two  possible  FINTIM  effects. 
Those  which  are  associated  with  a  small  FINTIM  and  those 
which  are  associated  with  an  excessively  large  FINTIM.  The 
net  effect  of  too  small  a  FINTIM  is  an  over  constraining  of 
the  problem,  which  leads  to  a  violation  of  problem 
constraints  and  excessive  computational  time. 

Two  things  happen  when  the  FINTIM  is  too  large.  The 
solution  adheres  more  to  problem  constraints  and  the 
computational  cost  decreases.  The  mission  objectives  of 
the  vehicle  (i.e.,  loitering  at  start  position),  are 
significant  considerations,  which  FINTIM  selection  must 
take  into  account.  Therefore,  FINTIM  is  a  critical 
parameter  which  effects  problem  solution  and  also,  when 
chosen  correctly,  significantly  reduces  computer 
computational  costs.  The  selection  of  FINTIM  poses  an 
important  problem  which  requires  solving  in  view  of  high- 
level  vehicle  objectives. 

One  guideline  for  selecting  FINTIM  is  to  select  it 
based  on  the  time  required  to  achieve  a  solution  while 
transversing  an  obstacle-free  field,  then  arbitrarily 
increase  FINTIM  to  allow  for  obstacle  avoidance.  The 
following  results  using  program  options  533  points  out  the 
importance  of  choosing  a  correct  FINTIM.  As  can  be  seen  in 
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Figure  5.1,  FINTIM  can  adversely  affect  the  problem 
solution  if  the  time  allotted  is  not  large  enough  to 
achieve  the  desired  result.  When  FINTIM  is  chosen  to  be 
6.0  non-dimensional  time  units  (NTU) ,  the  avoidance  zone 
constraint  for  zone  3  is  violated  and  the  desired  depth  of 
17.425  feet  is  exceeded.  When  FINTIM  is  increased  to  7.0 
NTU,  avoidance  zone  constraints  are  violated  for  zone  1  and 
zone  3  and  the  desired  depth  is  not  achieved.  However,  the 
severity  of  the  violations  are  not  as  blatant.  When  FINTIM 
is  increased  to  8.0  NTU,  the  desired  problem  solution  is 
obtained.  Table  3  presents  the  computational  costs 
associated  with  each  FINTIM  selection.  Note  that  the 
optimization  problem  is  easier  with  more  maneuvering  time, 
therefore  the  computational  cost  is  less. 


TABLE  3.  FINTIM  COMPUTATIONAL  COST 

FINTIM  TIME  (sec) 

6.0  95.06 

7.0  76.75 


8.0 


32.43 


VI 
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In  programming  for  three  dimensions,  we  not  only 
optimize  the  problem  to  obtain  bow  plane  and  stern  plane 
commands,  but  we  also  optimize  to  obtain  the  rudder 
commands.  In  order  to  achieve  the  desired  result,  it  was 
necessary  to  increase  the  number  of  design  variables  for 
the  rudder  in  the  linear  model.  The  problems  discussed 
previously,  have  all  been  solved  using  ten  discretizations 
(design  variables)  for  the  stern  plane  and  bow  plane 
inputs.  In  the  three-dimensional  work,  the  number  of 
design  variables  were  arbitrarily  increased  to  twenty 
(less  discretizations  would  not  achieve  a  satisfactory 
result) . 

A.  SIDE  CONSTRAINTS 

Additional  constraints  were  added  to  the  problem  in 
order  to  ensure  reasonable  vehicle  control  surface 
reactions.  The  maximum  rudder  angles  were  set  at  plus  or 
minus  thirty  degrees.  In  order  to  ensure  this,  the  side 
constraint  approach  was  invoked.  These  values  were 
assigned  to  the  Design  Variable  Lower  Bound  (VLB)  and  the 
Design  Variable  Upper  Bound  (VUB)  ADS  parameters. 
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B.  EQUALITY  CONSTRAINTS 


Six  additional  equality  constraints  were  needed  to 
achieve  the  desired  y  position  and  the  desired  vehicle 
condition  (yaw  and  roll)  at  the  desired  end  condition. 

C.  CONSTRAINT  SCALING 

Sanders  [Ref.  19]  points  out  that  constraint  weighting 
is  important  in  achieving  the  desired  results.  This  is 
even  more  crucial  in  a  three-dimensional  problem  solution 
because  of  the  increased  number  of  constraints  on  yaw, 
roll,  rudder  control  and  y  positioning.  It  appears  that 
problem  sensitivity  to  constraint  weighting  is  also 
increased.  In  order  to  achieve  the  desired  solution 
result,  it  was  necessary  to  adjust  constraint  scaling 
factors  until  all  constraint  conditions  were  satisfactorily 
obtained.  Table  4  shows  the  constraint  scaling  factors 
used  in  the  full  three-dimensional  linear  model. 


TABLE  4.  CONSTRAINT  SCALING  FACTORS 


CONSTRAINT 

SCALING  FACTOR 

depth 

0.5 

y  position 

2.5 

pitch 

1.0 

yaw 

1.0 

roll 

1.0 

minimum  depth 

1.0 
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D.  LINEAR/NONLINEAR  DYNAMICS 


Figure  6.1  illustrates  the  nonlinear  model  behavior 
when  using  control  inputs  for  bow  plane,  stern  plane,  and 
rudder  from  the  optimized  linear  model.  It  is  evident  that 
these  commands  are  invalid  for  the  full  scale  nonlinear 
model  since  the  final  objective  state  is  not  closely  met. 
Therefore,  the  essential  dynamics  of  the  linear  model  are 
not  valid  in  three  dimensions  as  might  be  suggested  from 
the  results  of  the  previous  study.  However,  even  though 
the  control  surface  inputs  are  invalid,  the  vehicle  state 
trajectory  is  valid  because  the  path  chosen  achieved  the 
desired  result.  For  a  desired  position  of  y=40.0  feet  and 
depth  =-20.0  feet,  the  obtained  result  was  y=4 0.269  feet 
with  a  depth  of  -20.699  feet.  These  values  can  be  fine 
tuned  by  varying  the  scaling  factors.  Figures  6.2  and  6.3 
illustrate  the  control  inputs  to  the  linear  model  and 
Figure  6.4  illustrates  the  linear  model  response  to  those 
inputs . 
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Figure  6.1  Nonlinear  Model  Maneuver  with  Linear  Control  Inputs 


Figure  6.2  Bow  and  Stern  Plane  Control  Inputs 


Figure  6.3  Rudder  Control  Input 


E.  THREE-DIMENSIONAL  COMPUTATIONAL  COSTS 

Table  5  compares  the  virtual  machine  time  of  the  full 
scale  linear  model  with  no  obstacles  as  compared  to  the 
full  scale  nonlinear  model  with  no  obstacles. 

TABLE  5.  LINEAR  VS.  NONLINEAR  COMPUTATIONAL  COSTS 

LINEAR  NONLINEAR 

(sec)  (sec) 

DIVE  PLANE  ONLY  14.23  108.81 

FULL  3D  130.34  370.61 
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VII 


As  previously  mentioned,  in  order  to  validate  the 
selected  optimization  configuration,  fixed  obstacles  were 
placed  at  various  positions  in  the  AUVs  field  of  view  with 
1.0  foot  radius  avoidance  zones  around  the  obstacles. 
Figure  7.1  to  7.8  illustrate  the  paths  chosen  by  the  311 
algorithm  to  avoid  the  obstacles  and  their  avoidance  zones 
for  various  obstacle  positions. 

The  moving  obstacles  were  simulated  using  rectilinear 
average  velocity  equations  of  the  form: 

s  =  Vt 

where: 

s  =  position  of  obstacle  (X  and/or  Y) 
v  =  constant  velocity 
t  =  time  of  travel 

Figures  7.9  to  7.11  present  the  distance  between  the  AUV 
and  the  moving  obstacle (s)  as  a  function  of  time.  As 
seen,  the  algorithm  chooses  a  path  in  both  cases  which 
avoids  impact.  Table  6  presents  the  computational  cost 
comparison  of  various  obstacle  case(s).  Some  three- 
dimensional  linear  and  non-linear  state  trajectory  results 
were  presented  earlier. 
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Figure  7.1  One  Obstacle  Solution 
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Figure  7.2  Two  Obstacles  Solution 


Figure  7.3  Three  Obstacles  Solution 


Figure  7 . 4  Four  Obstacles  Solution 
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Figure  7.5  Five  Obstacles  Solution 


Figure  7.6  Six  Obstacles  Solution 


Figure  7.8  Seventeen  Obstacles  Solution 


Figure  7.9  One  Moving  Obstacle  Solution 


Figure  7.10  Two  Moving  Obstacles  Solution  (Obstacle  1) 


Figure  7.10  Two  Moving  Obstacles  Solution  (Obstacle  2) 
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TABLE  6.  PROGRAM  311  COMPUTATIONAL  COST-2 D 


NUMBER  OF  OBSTACLES 
0 

1  (moving) 

2  (moving) 
17  (fixed) 


TIME  (sec) 
14.23 
23.88 
24.68 
87.52 
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VIII.  CONCLUSIONS  AND  RECOMMENDATIONS 

A.  CONCLUSIONS 

The  following  conclusions  can  be  drawn  from  this 
feasibility  study: 

1.  Optimal  control  theory  is  a  feasible  method  for 
determining  obstacle  avoidance  paths  in  the  presence  of 
fixed  and  moving  obstacles. 

2.  The  introduction  of  obstacle  constraints  into  the 
algorithm  increases  the  computer  computational  costs. 

3.  The  full  linear  model  control  inputs  are  not 
compatible  with  the  full  nonlinear  model.  When  linear 
commands  were  placed  in  the  nonlinear  model,  the  vehicle's 
end  condition  was  inconsistent  with  the  desired  end 
condition. 

4 .  The  best  general  algorithm  for  the  two  dimensional 
case  was  determined  based  on  the  ability  of  the  algorithm 
to  solve  a  variety  of  obstacle  problems  with  a  shortened 
FINTIM. 

5.  Scaling  factors  can  be  critical  in  achieving  a 
desired  problem  solution. 

6.  In  order  to  achieve  a  solution  in  three 
dimensions,  it  is  necessary  to  increase  the  number  of 
design  variables  for  certain  vehicle  control  inputs. 
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7. 


FINTIM  is  a  critical  variable  whose  value  can 


change  the  solution  to  a  specific  problem. 


B.  RECOMMENDATIONS 

1.  Find  a  procedure  to  estimate  the  optimal  scaling 
factors  for  end  constraints. 

2.  Study  the  discretization  factors  in  the  three- 
dimensional  case(s). 

3.  Study  the  selection  of  FINTIM  in  the  two-dimensional 
case  and  in  the  three-dimensional  case.  Vehicle 
mission  objectives  should  be  considered  in 
establishing  guidelines. 

4.  Develop  the  optimization  of  the  three-dimensional 
nonlinear  model. 

5.  Develop  the  algorithm  to  include  optimization  of 
the  propeller  rpm  control  input. 

6.  Develop  the  program  for  efficient  programming  in  a 
microprocessor . 

7.  Determine  if  the  general  algorithm  recommended  in 
this  thesis  is  also  applicable  for  three-dimensional 
obstacle  avoidance  cases. 
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APPENDIX 


PROGRAMS 


This  appendix  contains  the  four  primary  programs  that 
were  used  for  this  feasibility  study.  They  were: 

1.  OBST  DSL  -  This  is  the  state  linear  2D  model  used 
for  the  2D  analysis. 

2.  TLO  DSL  -  This  is  the  ADSL  program  used  to  optimize 
the  full  scale  linear  model  of  bow  plane,  stern 
plane  and  rudder  control  vectors. 

3 .  TNLO  DSL  -  This  is  the  ADSL  program  used  to  optimize 
the  full  scale  nonlinear  model  for  timing  comparison 
studies . 

4.  TLNLO  DSL  -  This  is  the  ADSL  program  used  to 
optimize  the  full  scale  linear  model  for  bow  plane, 
stern  plane  and  rudder  control  inputs  and  with 
simulation  of  the  full  scale  nonlinear  model. 
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FILE:  OBS 


DSL 


A1 


TITLE  LINEAR  AUV  DYNAMIC  PATH  PLANNER  FOR  VERTICAL  PLANE  MOTION 
x  SEPARATED  BOW  AND  STERN  PLANE  CONTROL  NON-DIMENSIONAL 
x 

X  2D  STATIONARY  OBSTACLES 
x 
x 
X 

xxxxxxxxxxxxxxxxxxxxxxx  ADSL  SET  UP  xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx 

X 

FIXED  ISTRAT,  IOPT,  IONED,  IPRINT,  INFO,  IGRAD,  NDV,  NCON 
FIXED  IDG,  NGT ,  IC,  NRA ,  NCOLA,  NRWK,  IWK,  NRIWK,  0 
D  DIMENSION  AW(42,42) 

ARRAY  WKC5000),  IWK<500) 

ARRAY  DX(  21 ) ,  VLBC21),  VUB<21),  GWC  05) ,  DFC21),  IDGL05),  IC(OS) 

PARAM  NRA=42,  NC0LA=42,  NRWK=5000,  NRIWK=500 
PARAM  IGRAD=0 ,  INF0=0,  NDV=20,  NCON=05,  NGT=05 
TABLE  DX( 1-2) =2* . 0 ,  DXC 3-21 ) =1 9X0 . ,  IDG(l-4)=4X-l 

TABLE  VLB(1-9)=9X-. 17452,  VLBC 1 1-19 ) =  9x- . 2443, VLBC 10 )  =  0 . ,VLB( 20-21 )  =  0 . 
TABLE  VUB(1-9)=9X. 17452,  VUBC 1 1-19) =9X . 2443, VUBC 1 0) =0 ., VUBC 20-21 ) =0 . 
TABLE  IDGC5)=01X1 

PARAM  ISTRAT=3, I0PT=1 ,  IONED=l,  IPRINT=0000 
INCON  U=0.0 
METHOD  RECT 

CONTROL  FINTIM=7 . 0,  DELT= . 1 
PRINT  XPOS,ZPOS 

XRINT  DS,DB, DEPTH, PITCH, XPOS,ZPOS,DT 
x 

xxxxxxxxxxxxxxxxxxxxxxxxxxxxDSL  MODEL  SET  UPxxxxxxxxxxxxxxxxxxxxxxxxxxx 

x 

x 

x  EQUILIBRIUM  CONDITION  IS  CONSTANT  SPEED  ( NON-DIMENSIONAL  I ZED)  BY 

X  UO  =  6  FT/SEC 

CONST  U0=1.0,A=10,B*11,C=12,D=13,E=14,F=15,G=16,H=17 
XONST  X0BS1=36 . 0 , Z0BS1=-12 . 0 , XOBS2=72 .5,Z0BS2=-5.S2 
XONST  X0BS3=90 .0,ZOBS3=-16 . , X0BS4=115 . , Z0BS4=-17 . 
xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx 

CONST  X0BS1=34 . 85,Z0B51=-2 .8297 
CONST  XOBS2=83.64,ZOBS2=-12.960 
CONST  XOBS3=122.5,Z0BS3=-2.90 
CONST  XOBS4=105. 0,  Z0BS4=-6.74 
CONST  X0BS5*59 . 245,  ZOBS5  =  -7 . 2363 
CONST  XOBS6=35 . 0,  Z0BS6=-14.58 

CONST  XOBS7=17 .5,  Z0BS7=-8.74 
CONST  X0BS8=52 . 5 , ZOBS8=-ll .66 
CONST  X0BS9=69.7,Z0BS9=-10.155 
CONST  X0BS10=70 . 0,  ZOBS10=-2.9 
CONST  X0BS11=52 . 275,  ZOBSll=-6 . 1408 
CONST  XOBS12=52.275,ZOBS12=-5.21 
CONST  XOBS13=52 . 275,  ZOBS1 3=-6 . 8836 
CONST  XOBS14=50.0,  Z0BS14=-5.21 
CONST  X0BS15=54 . 55,  ZOBS15=-5.21 
CONST  X0BS16  =  57 .5,  Z0BS16=-8.74 
CONST  X0BS17  =  35 . 0 ,  Z0BS17  =  -8.74 
x 
x 
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CONST 

U0= 

CONST 

MA  = 

,  THETAO=  ,  Z0=  ,W0= 

,  IY= 

CONST 

CONST 

ZW  = 
ZDB- 

,  ZQ=  ,  ZQDOT= 

,  ZDS=  ,  Z0= 

,  ZWDOT * 

CONST 

CONST 

MW= 

MDB  = 

,  MQ=  ,  MQDOT  = 

,  MDS=  ,  MTHETA= 

,  MWDOT= 

INITIAL 

DSAVE1=  SORT  < (XPOS-XOBSl )*(XP0S~X0BS1 )+(ZPOS-ZOBSl )X(ZPO$-ZOBSl ) ) 
DSAVE2=SQRT ( ( XP0S-X0BS2 )*(XP05-X0BS2 )+(ZP0S-Z0BS2)*(ZP0S-Z0BS2 ) ) 
DSAVE3=SQRT ( (XP0S-X0BS3 )*( XP0S-X0BS3 )+(ZPQS-ZOBS3)*(ZPOS-ZOBS3) ) 
DSAVE4=SQRT( (XP0S-X0BS4)*(XP0S-X0BS4 )+(ZP0S-Z0BS4)*(ZP0S-Z0BS4) ) 
DSAVE5=SQRT ( (XP0S-X0BS5 )*( XP0S-X0BS5)+(ZP0S-Z0BS5)*(ZP0S-Z0BS5) ) 

D5AVE6=SQRT ( (XPOS-XOBS6 )*(XPOS-XOBS6 )+(ZP0S-Z0BS6 )X(ZPOS-ZOBS6 ) ) 
DSAVE7=SQRT( (XP0S-X0BS7 )X( XP0S-X0BS7 )+(ZP0S-Z0BS7 )X(ZPOS-ZOBS7 ) ) 
DSAVE8=SQRT ( ( XP0S-X0BS8 )*( XPOS-XOBS8 )+ (ZP0S-Z0BS8 )x(ZP0S-Z0BS8 ) ) 
DSAVE9=SQRT ( CXPOS-XOBS9 >XC XPOS-XOBS9 )+( ZP0S-Z0BS9 )x(ZP0S-Z0BS9 ) ) 
DSAVEA=SQRT  C (XPOS-XOBSIO )X( XPOS-XOBS1 0 )+ . . . 
(ZPOS-ZOBSIO)X(ZPOS-ZOBSIO) ) 

DSAVEB=SQRTC ( XPOS-XOBS1 0 )x( XPOS-XOBS1 0 )+ . . . 
(ZPOS-ZOBSlCn*CZPOS-ZOBS10» 

DSAVEC  =  SQRTUXPOS-XQBS10)x(XPOS-XOBS10)+. . . 
(ZPOS-ZOBSIO)X(ZPOS-ZOBSIO)) 
DSAVED=SQRT(CXPOS-XOBS10)*CXPOS-XOBSlO)+. . . 
CZPOS-ZOBSIO)X(ZPOS-ZOBSIO) ) 
DSAVEE=SQRTUXPOS-XOBS10)x(XPOS-XOBS10)+. . . 
(ZPOS-ZOBSIO)X(ZPOS-ZOBSIO)) 

DSAVEF=SQRT ((XPOS-XOBSIO )X( XPOS-XOBSIO )+ . . . 
(ZPOS-ZOBSIO)X(ZPOS-ZOBSI 0 ) ) 

DSAVEG=$QRT( (XPOS-XOBSIO )x( XPOS-XOBSIO )+ . . . 
(ZPOS-ZOBSIO)X(ZPOS-ZOBSIO) ) 

DSAVEH=SQRT ( ( XPOS-XOBSIO )»( XPOS-XOBSIO )+ . . . 
(ZPOS-ZOBSIO)X(ZPOS-ZOBSIO)) 

ORDDEP  =1.0 
A1=-ZW 

A2=(MA-ZHD0T) 

A3=-CZQ+MA) 

A4=-ZQD0T 

B1=-MW 

B2=-MWD0T 

B3=-MQ 

B4  =  ( I Y-MODOT ) 

B5  =  -MTHET  A 
Cl =ZDS 
C2=ZDB 
C3=MDS 
C4=MDB 

C5=C3-(B4XC1)/A4 

C6=C4-(B4XC2)/A4 

D1=B2-(B4XA2)/A4 

D2=B1-(B4»A1VA4 

D3=B3-(B4*A3)/A4 

CALL  DADS ( INFO » ISTRAT, IOPT, IONED, IPRINT , I  ;RAD> NDV , NCON , DX, . . . 

VLB,VUB,DBJ,GW,IDG,NGT,IC,DF,AW,NRA,NCOLA,WK,NRWK, . . . 
IWK,NRIWK) 

IF(INFO.EQ.O)  THEN 

SAVE  THETDD, THETAD, THETA, W,Z, DEPTH, PITCH, DS, TB, BOWANG, STNANG 
ELSE 
ENDIF 

IF(INFO.EQ.O)  DELPRT  =  0.2 
I F( INFO . EQ . 0 )  DELPLT  =  0.2 
DERIVATIVE 
* 
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X***  X  XXX 


1HETDD=<1/A4)k(C1*DS+C2*DB-A1*W-A2*WD0T-A3*THETAD) 

WDOT  =  (1/DI >*CC5XDS  +  C6*DB  -  D2XW  -  D3XTHETAD  -  B5*THETA) 
THETAD=  INTGRL (THETAO, THETDD) 

THETA  =  IHTGRL (THETAO, THETAD) 

W  =  INTGRL(WO,WDOT) 

Z  =  INTGRL(ZO,W-UOXTHETA) 

D£PTH=-Z 

PITCH=THETA/. 01745 
BOWANG=(DB'. 01745) 

STNANG=(DS/. 01745) 

INTGRD  =  aW*W+(Z-ORDDEP)*CZ-ORDDEP>+. . . 

THETAD*THETAD+THETA*THETA) )  +  C DSXDS+DBXDB) 

OBJ1  =  INTGRKO. ,  (0.5)XINTGRD) 

OBJ  =  OBJ1 

x 

DYNAMIC 

RN=TIME/(FINTIM/10 .-DELT/10000 . ) 

0=INT(RN)+1 
IF(O.EQ.ll)  0=10 
x 

x  ADDITIONALLY  THE  PLANES  SHOULD  BE  AT  EQUILIBRIUM  SO  THE 
x  VEHICLE  MILL  PROCEED  AT  THIS  NEW  DEPTH  WITHIN  SOME  TOLERANCE 

x 

DS=DX(0) 

DB'-DXC  1 0+0 ) 

IFIO.GE. 10)  DS=0. 

IF(O.GE.IO)  DB=0 . 

CONSTRAINTS  FOR  A  DIVE 

ORDERED  DEPTH  =  ORDDEP 
GWC 1 )  =(Z-0RDDEP)/2. 

GWC2)  = ( ORDDEP-Z )/2 . 

AUVS  FINAL  STATE  MUST  BE  LEVEL  FLIGHT  AS  FOLLOWS 
GW( 3)  =  THETA  XlO. 

GW( 4 )  =  -THETAX10. 

GW(5)=-ZX100. 

X-Z  POSITIONING  FOR  OBSTACLE  AVOIDANCE 

XP0S=17.425*TIME 
ZP0S=-ZX17 .425 
DT=TIMEX20./FINTIM 


*  K 


AVOIDING  THE  OBSTACLE 

DI3T1=SQRTL1XPOS-XOBS1)XIXPOS-XOBS1)+LZPOS-ZOBS1)XLZPOS-ZOBS1)) 
DIST2  =  S5RT L L XP0S-X0BS2 )x(XPQS-X0BS2)+LZP0S-Z0BS2 )x{ ZPDS-Z0BS2 ) ) 
DIST3=SQRTLLXPOS-XOBS3)XLXPOS-XOBS3)+LZPOS-ZOBS3)xLZPOS-ZOBS3)) 
DIST4=SQRTLL  XP0S-X0BS4)XL  XP0S-X0BS4 )  +  LZP0S-Z0BS4 )XL  ZP0S-Z0BS4 ) ) 
DIST5=SQRTLL XP0S-X0BS5 )X( XPOS-XOBS5)  +  LZP0S-ZQBS5 )XL  ZP0S-Z0BS5) ) 
DIST6=SQRT LLXPOS-XOBS6 )X( XPOS-XOBS6 )+LZP0S-Z0BS6 )x(ZP0S-Z0BS6 ) ) 
DIST7=SQRT({ XP0S-X0BS7 ) XL  XP0S-X0BS7)  +  LZP0S-Z0BS7 )x( ZP0S-Z0BS7 ) ) 
DIST8=SQRT ((XPOS-XOBS8 )XL  XP0S-X0BS8 )  +  LZP0S-Z0BS8 )XL  ZP0S-Z0BS8 ) ) 
DI$T9=SQRT L (XPOS-XOBS9 )XLXP0S-X0BS9 )+LZPOS-ZOBS9 )*L  ZPOS-ZOBS9) ) 
DIST10=SQRT(L XPOS-XOBSIO) XL  XPOS-XOBS1 0)+LZPOS-ZOBS10 )x . . . 

czpos-zoBsion 

DIST11=SQRT ( LXPOS-XOBS11 )*LXPOS-XOBSll )+LZPOS-ZOBSll )x . . . 

C  ZPOS-ZOBS1 1 ) ) 

DIST12=SQRTLLXPOS-XOBS12)XLXPOS-XOBS12)+LZPOS-ZOBS12)X. . . 
CZP0S-Z0BS12)) 

DIST1 3=SQRT ( ( XP0S-X0BS13 )XL  XP0S-X0BS13)  +  LZP0S-Z0BS13)x . . . 
LZPOS-ZOBS13) ) 

DIST14=SQRTL LXP0S-X0BS14 )x(XPQS-X0BS14)+LZP0S-Z0BS14)x . . . 
(ZP0S-Z0BS14)) 

DIST15=S0RT( (XPOS-XOBS15)x(XPOS-XOBS15)+(ZPOS-ZOBS15)x. . , 
LZPOS-ZOBS15D 

DIST16=SQRT((XP0S-X0BS16)X(XP0S-X0BS16)+(ZP0S-Z0BS16)x. . . 
(ZP0S-Z0BS16) ) 

DIST17=SQRT(L XPOS-XOBS17 )X( XP0S-X0BS17 )+CZP0S-Z0BS17 )X . . . 

( ZPOS-ZOBS17 ) ) 

IF(DISTl.LT.DSAVEl)  DSAVE1=DIST1 
IFCDIST2.LT.DSAVE2)  DSAVE2=DIST2 
IF( DIST3.LT. DSAVE3 )  DSAVE3=DIST3 
IF( DIST4.LT. DSAVE4 )  DSAVE4=DIST4 
IF(DI ST5.LT. DSAVE5 )  DSAVE5=DIST5 
IFLDIST6 .LT.DSAVE6)  DSAV£6=DIST6 
IFLDIST7.LT. DSAVE7)  DSAVE7=DIST7 
IFLDIST2.LT. DSAVE8 )  DSAVE8=DIST8 
IFLDIST9.LT.DSAVE9)  DSAVE9=DIST9 
IFLDISTIO.LT. DSAVEA )  DSAVEA=DIST10 
IFLDIST11.LT. DSAVEB )  DSAVEB=DIST1 1 
IFLDIST12.LT. DSAVEC)  DSAVEC=DIST12 
IFLDIST13.LT. DSAVED )  D5AVED=DI5T13 
IFLDIST14.LT. DSAVEE )  DSAVEE=DIST14 
IFLDIST15.LT.DSAVEF)  DSAVEF=DIST15 
IFLDIST16 .LT.DSAVEG)  DSAVEG=DIST16 
IFLDIST17 .LT.DSAVEH)  DSAVEH=DIST17 
GW(6)=L9. -DSAVEl) 

GWL7)=L5.-DSAVE2) 

GWL8)=L3.-DSAVE3) 

GWL  9 )  =  L6 . -DSAVE4 ) 

GWL 10 ) =L 1 . -DSAVE5 ) 

GWC 1 1 )=L 1 .  -DSAVE6 ) 

GWL 12)= L 1 . -DSAVE7 ) 


53 


FILE:  OBS  DSL  A1 


GW(13)=(1 .-DSAVEB) 
GW(14)=(1 .-DSAVE9) 

Gl>){  15 )  =  (  1 .  -DSAVEA) 
GW(16)=(1 .-DSAVEB) 

GW( 17 )=(1 .-DSAVEC) 

GW( 18)s(l .-DSAVED) 

GWC 19 )  =  ( 1 . -DSAVEE) 
GW(20)=(1 .-DSAVEF) 

GN(21 )  =  (1 .-DSAVEG) 
GW(22)=(1 .-DSAVEH) 

TERMINAL 

IF( INFO . EQ . 0)  THEN 
PRINT*, DSAVE1 ,  DSAVE2 
PRINT*, DSAVE3, DSAVEA 
PRINT*, DSAVE5.DSAVE6 
PRINT*, DSAVE7,DSAVE8 
PRINT*, DSAVE9, DSAVEA 
PRINT*, DSAVEB, DSAVEC 
PRINT*, DSAVED, DSAVEE 
PRINT*, DSAVEF, DSAVEG 
PRINT*, DSAVEH 
ELSE 
ENDIF 

IF( INFO . EQ . 0)  CALL  ENDJOB 
CALL  RERUN 

* 

END 

STOP 
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FILE:  TLO  DSL  A1 


TITLE  RUN: 16-5  LINEAR  AUV  MODEL  /  STERN  PLANE  AND  BOW  PLANE  SEPARATED 

*  <1)  UPDATED : 0 A/ 16/8 8 

*  C2)  100.00  FT  DEPTH  CHANGE  IN  20  SEC 

*  (3)  RIGHT  OBJ  EQUATION 

*  (4)  ADS  CONSTRAINTS  ON  DEPTH  AND  PITCH 

*  (5)  OBSTACLE  FURTHER  DOWN  THE  TRAJECTORY  AND  ABOVE  IT 

*  (6)  CORRECT  OBSTACLE  AVOIDANCE  ROUTINE  ADDED 

XXXXXXXXXXXXXXXXXXXXXADSL  SET-UPxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx 
FIXED  ISTRAT,  IOPT,  IONED,  IPRINT,  INFO,  IGRAD,  NDV,  NCON 
FIXED  IDG,  NOT ,  IC,  NRA,  NCOLA,  NRWK,  IWIC,  NRIWK,  0,  H,D,C,P 
D  DIMENSION  AWC42,42) 

ARRAY  NKC  4000 ) ,  IWKC1000) 

ARRAY  DX( 40 ) ,  VLBC40),  VUBC40),  GWC11),  DF(41),  IDG(ll),  ICC11) 

PARAM  NRA=42,  NC0LA=42,  NRWK=4000,  NRIWK=10Q0 
PARAM  IGRAD=0 ,  INF0=0,  NDV=40,  NCON=ll,  NGT=11 
TABLE  DX<1-2)*2X.0,DX(3-21)=19X0. ,  IDGC1-10)=10*-1 
TABLE  DXC  22-40 ) =19X0 . 

TABLE  IDGC7-0)=1*1 

TABLE  VLB(1-9)=9X-. 17452,  VL BC 1 1-1 9 )=09*- . 2443, VLBC 1 0 )=0 . , VLBC 20 ) =0 . 

TABLE  VUBC1-9)=9X. 17452,  VUBC 11-19 ) =9X . 2443, VUBC 1 0) =0 . , VUB( 20 ) =0 . 

TABLE  VLBC 21-39 )=19*-. 523627, VUBC 21-39 >=19*. 523627, VUBC 40-41 )=0 . 

TABLE  VLBC40-41)=0 . 

PARAM  ISTRAT=3,  IOPT=l,  IONED=l,  IPRINT=0000 
INCON  H=0,  0BS1=0. ,YZONE=0. 

METHOD  RECT 

CONTROL  FINTIM=21 .  ,  DELT=.10 

XRINT  XPOSM, YPOSM, DEPTH, THETAM, PHIM, PSIM, DSM, DBM, DRM 
PRINT  XPOSM, YPOSM, DEPTH 

XRINT  DSM, DBM, DRM, PITCHM, XPOSM, YPOSM, DEPTH, NDT 

XRINT  THETAD,W, DEPTH, PITCH, XPOS, DEPTH, NBX,NDZ, NDT 

XAVE  THETA, W,Z, DEPTH, PITCH, DS , DB, BOWANG, STNANG 

XRAPHC DE=TEK618 )  TIME, DS 

XRAPHC  DE=TEK6 18 )  TIME, DEPTH 

XRAPHC DE=TEK6 18 )  TIME,WDOT 

XRAPHC  DE=TEK618 )  TIME,W 

XRAPHC  DE=TEK6 18 )  TIME.THETDD 

XRAPHC  DE=TEK6 18 )  TIME, THETAD 

XRAPHC  DE=TEK618 )  TIME, THETA 

XRAPHC  DE  =  TEK618 )  TIME, PITCH 

XRAPHC  DE=TEK618 )  TIME, BOWANG 

XRAPHC  DE=TEK618 )  TIME, STNANG 

xxxxxxxxxxxxxxxxxxxd  S  L  MODEL  FOR  LINEAR  SIMULATION  xxxxxxxxxxxxxxxxxxxx 
X  LINEAR  MODEL  ONLY 

D  COMMON/BLOCK1/  MMINVC6 , 6 ) ,  MMC6,6),  AAC12,12),  BBC6,6) 

D  COMMON/ BL0CK2/  BC 6 , 6 ) , AC  12 , 12 ) ,  UMODC 6 ) , GKKC6 , 21 ) 

D  COMMON/ BLOCK3/  FC12),  FPC6 ) ,  UCFC4) 

D  COMMON/ BL0CK4/  G4C 4) , GK4C 4 ) , BRC 4 ) , HHC 4 ) 

D  COMMON/ BL0CK5/  XDOTC 12 ) , XDOTXC 12 ) ,  XD0TUC6  ) 

FIXED  N, IA, IDGT ,IER,LAST,J,K,M,JJ,KK,I 
INTEGER 

ARRAY  WKAREAC54) ,  XC12) 
x 


x 

CONST 


x 


x 


x 

CONST 


LONGITUDINAL  HYDRODYNAMIC  COEFFICIENTS 


XPP  =  7.03E-3 
XUDOT=-7 . 58E-3 
XQDS=  2.61E-2 
XWW  =  1.71E-1 
XDSDS=-1 . 16E-2 
XWDSN=3 . 46E-3 


XQQ  =  -1.47E-2 
XWQ  =  -1.92E-1 
XQDB=  -2.6E-3 
XVDR=  1.73E-3 
XDBDB=-8 . 07E-3 
XDSDSN=-1 , 62E-3 


, XRR  =  4.01E-3 
,XVP  =  -3.24E-3 
, XRDR=  -8.18E-4 
, XWDS=  4.6E-2 
, XDRDR=-1 . 01 E-2 


, XPR  =  7 .64E-4, . 
,  XVR  =  1 .89E-2, . 
,XVV  =  5.29E-2, . 
, XWDB=  9 . 66E-3, . 
, XQDSN  =  1 . 96E-3,  . 
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LATERAL  HYDRODYNAMIC  COEFFICIENTS 


x 

x 


CONST  YPDGT=1 .27E-4  , YRDOT=l . 24E-3  ,YPQ  =  4.125E-3 

YVD0T=-5 . 55E-2  ,YP  =  3.055E-3  ,YR  =  2.97E-2 
YWP  =  2.35E-1  , YWR  =  -1.8SE-2  ,YV  =  -9.31E-2 
YDR  =  2.73E-2  ,CDY  =  3.5E-1 
X 

x  NORMAL  HYDRODYNAMIC  COEFFICIENTS 


» YQR 
,YVQ 
, YVW 


=-6 . 51 E-3 ,  .  . 
=  2.36E-2,  .  . 
=  6 .84E-2,  .  . 


x 

CONST 


x 

X 


X 

CONST 


x 


x 


x 

CONST 


x 

X 


X 

CONST 


x 

X 


X 

CONST 


x 

CONST 

CONST 


ZQD0T=-6.81E-3  ,ZPP  =  1.27E-4 
ZMDOT =-2 .  43E-1  ,ZQ  =  -1.35E-1 
ZW  =  -3.02E-1  ,ZVV  =  -6.84E-2 
ZQN  =  -2.88E-3  ,ZWN  =  -5.07E-3 

ROLL  HYDRODYNAMIC  COEFFITIENTS 

KPDOT=-l . 01E-3  ,  KRDOT=-3 . 37E-5 
KVDOT=l . 27 E-4  ,  KP  =  -1.1E-2 

KWP  =  -1.27E-4  ,  KWR  =  1.39E-2 
KPN  =  -5.73E-4,  KDB  =  6.94E-3 


ZPR  *  6.67E-3  ,ZRR  =-7.35E~3,.. 
ZVP  =  -4.81E-2  , ZVR  =  4.55E-2,.. 
ZDS  =  -7 . 255E-2, ZDB  =-2.58E-2,  .. 
ZDSN=  -1 . 015E-2, CDZ  =1.0 


KPQ  =  -6.93E-5  , KQR  =  1.68E-2,.. 
KR  =  -8.41E-4  , KVQ=-5 . 115E-3, . . 
KV  =  3.055E-3  ,KVW  =-1.87E-l,.. 


PITCH  HYDRODYNAMIC  COEFFICIENTS 

MQDOT  =  -1.68E-2,  MPP  =  5.26E-5  ,MPR  =  5.04E-3  ,MRR  =-2.86E-2,.. 
MW DOT  =  -6.81E-3,  MQ  =  -6.86E-2  ,MVP  =  1.18E-3  ,MVR  =  1.73E-2,.. 
MW  =  9.86E-2  ,  MW  =  -2.51E-2  ,MDS  =  -4.12E-2  ,MDB  =  6.94E-3,.. 
MQN  =  -1.64E-3  ,  MWN  =  -2.88E-3  ,MDSN  =  -5.76E-3 


YAW  HYDRODYNAMIC 

NPDOT=-3 . 37E-5  , 
NVD0T=1.24E-3  , 
NWP  =  -1.75E-2  , 
NDR  =  -1.29E-2 


COEFFICIENTS 

NRDOT=-3 . 4E-3 
NP  =  -8.405E-4 
NWR  =  7.35E-3 


> NPQ  =  -2.11E-2 
,NR  =  -1.64E-2 
,NV  =  -7.42E-3 


,NQR  =  2.75E-3, . . 
, NVQ  =-9.99E-3,  .  . 
,  NVW  =-2.67E-2, . . 


MASS  CHARACTERISTICS  OF  THE  FLOODED  MARK  IX  VEHICLE 


WEIGHT  =  15900 
YG  =  0.0 
IX  =  1760 
IYZ  =  -  7.0 
L  =  17.425 
AO  =  1.57 
DEGRUD=  10.0 


,  BOY  =  15900 
,  ZG  =  0.061 
,  IY  =  9450 
,  IXY  =  -  7.0 
,  RHO  =  1.94 
, KPROP  =  0.0 
,DEGSTN=  0.0 


» VOL  =  248.44 
,XB  =-0.1 
,IZ  =  10700 
,YB  =  0.0 
»  G  =  32.2 
, NPROP  =  0.0, 


, XG  =-0.1 
, ZB  =0.023 
,IXZ  =  -6.65 

, NU  =  1.5E-5 
X1TEST=0 . 01 


XOBS1 =36 . 0 
Z0BS1=  -12.0 
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*  *  * 


* 


V 


INPUT  INITIAL  CONDITIONS  HERE  IF  REQUIRED 


x 

INITIAL 


x 

X 

X 

X 


DSAVE1=SQRT ( CXPOSM-XOBS1 )*CXPOSM-XOBSl )+ . . . 

CZPOSM-ZOBSl)xCZPOSM-ZOBSl) ) 
INITIALIZE  ALL  MATRICES  AND  ARRAYS  TO  ZERO 


NOSORT 

0RDDEP=20 . 

YORD=  40. 

D=0 

H=H+1 

IF  CH.EQ.l)  THEN 

N  =  6 

DO  2  J  =  1,N 
JJ=  J+N 
DO  1  K  *  l.N 
KK=  K+N 
KKK  =  KK  +  N 
MMINV  C  J ,  K)  =  0.0 
XCJ)  =  0.0 
X(JJ)  =  0.0 
XDOTCJ)  =  0.0 
XDOTCJJ)  =  0.0 
XDOTX(J)  =  0.0 
XDOTXCJJ)  =  0.0 
XDOTUCJ)  =  0.0 
UMOD(J)  =0.0 
MMCJ.K)  =  0.0 
BBCJ,K)  =  0.0 
BCJ.K)  =  0.0 


AACJ,K)=  0.0 
AACJJ,KK)=  0.0 
AA( J , KK)=  0.0 
AAC JJ»K)=  0.0 
AC  J , KK)  =  0.0 
AC JJ, K)  =  0.0 
AC  J , K )  =  0.0 
AC  J J , KK)  =  0.0 
GKKC  J ,  K  )  =  0.0 
GKKC  J , KK) =0 . 0 
GKKC J,KKK)=0.0 
CONTINUE 
CONTINUE 

INPUT  THE  LINEARIZATION  POINT  PARAMETERS 

UO  =6.0 
VO  =  0.0 
WO  =  0.0 
PO  =  0.0 
QO  =  0.0 
RO  =  0.0 
PHIO  =  0.0 
THETAO  =0.0 
PSIO  =  0.0 
SUM  =  0.0 
JFLAG  =  0 
I  FLAG  =  0 
KFLAG  =  0 
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*  INPUT  THE  MODEL  STATES  INITIAL  CONDITIONS 
x 

UM  =  6.0 
VM  =  0.0 
WM  =  0.0 
PM  =  0.0 
QM  =  0.0 
RM  =  0.0 
XPOSM  =  0.0 

yposm  =0.0 

ZPOSM  =0.0 
PHIM  =  0.0 
THETAM  =0.0 
PSIM  =  0.0 
X 
x 

x  INPUT  THE  VEHICLE  INITIAL  CONDITIONS 

x 

x 

X  INITIALIZE  THE  CONTROLS 

x 

DBOY=  0.0 
DR  =  0 . 0 
DS=  0.00000 
X  DSM  =  0 . 

X  DBM=0. 

DB  =  0 . 000000 
DRM=  0  .  0 
DRPM  =  0  . 

RPM  =  500.00 
LATYAH  =0.0 
NORPIT  =0.0 
x 

X 

MASS  =  WEIGHT/G 
x 

DIVAMP  =  DEGSTNXO. 0174532925 
RUDAMP  =  DEGRUDXO. 0174532925 
x 

x  THE  LINEAR  PROPULSION  MODEL 

x 

X 

x  ETA  =  0.012XRPM/UO 

ETA  =  1.0 
RE  =  UOXL/NU 

CDO  =  .00385  +  (1 .296E-17)X(RE  -  1.2E7)XX2 

CT  =  0.008XLXX2XETAXABS(ETA)/CA0) 

CT1  =  0.008XLXX2/CAO) 

EPS  =  -1.0+(SQRT(CT+1.0)-1.0)/(SQRT(CT1+1.0)-1.0) 
XPROP  =  CDQX(ETAXAB5(ETA)  -  1.0) 

x  N=6 

*  DO  15  J  =  1,N 

*  DO  10  K  =1 , N 

*  MMINVCJ, 10  =  0.0 

*  MM(J,K)  =  0.0 

*0  CONTINUE 

*5  CONTINUE 

x 

*  CALCULATE  THE  MASS  MATRIX 
x 

MM( 1,1)  =  MASS  -( (RH0/2)*CLXX3)XXUD0T) 

MM( 1,5)  =  MASS#ZG 
MM ( 1 >  6 )  =  -MASSXYG 
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*****  *  *  IS}  ** 


MM(2,2)  =  MASS  -( ( RH0/2)x( L XX3)XYVD0T ) 

MM ( 2 , 4 )  =  -MASSXZG  - (( RHO/2 )x( LXX4 )XYPDOT) 
MMC2, 6 )  =  MASSXXG  -  < CRH0/2)X( Lxx4)xYRD0T) 

MM(3»  3)  =  MASS  -  ( ( RH0/2)X( LXX3)XZWD0T) 
MMC3,4)  =  MASSXYG 

MMC 3/ 5 )  =  -MASSXXG  -( CRH0/2)X(LXX4)XZQD0T) 

MM<4, 2)  =  -MASSXZG  -  ( <RH0/2)X(LXX4)XKVD0T> 
MM(4, 3)  =  MASSXYG 

MM( 4, 4 )  =  IX  -  ((RH0/2)XCLXX5)XKPD0T) 
MMC4,5)  *  -IXY 

MM(4,6)  =  -IXZ  -URH0/2)X(LXX5)XKRD0T) 
MM<5,1)  *  MASSXZG 

MMC5,3)  =  -MASSXXG  -C (RH0/2)X( LX*4)XMWD0T) 
MM( 5/ 4 )  =  -IXY 

MMC5.5)  =  IY  -((RH0/2)X(LXX5)XMQD0T) 

MMC5,6)  =  -IYZ 

MMC6.1)  =  -MASSXYG 

MM( 6  <  2 )  =  MASSxXG  -( f RH0/2)x( L xx4 )XNVDOT) 
MM(6,4)  =  -IXZ  -  ((RH0/2)X(LXX5)XNPD0T) 
MM(6,5)  =  -IYZ 

MM(6 , 6 )  =  IZ  -  ( ( RHO/2 )*( LXX5 )XNRDOT ) 


LAST  =  NXN+3XN 
DO  20  M  =  1 , LAST 
WKAREA(M)  =  0.0 
CONTINUE 

IER  =  0 
IA  =  6 
IDGT  =  4 

CALL  LINV2F(MM,N,IA,MMINV, IDGT, WKAREA, IER) 
CALCULATE  THE  A  MATRIX  FOR  THE  LINEAR  MODEL 


A(U)  =  RHO/2XLX*3X(XODSXDSXQO+XQDB/2XDBXQO+XRDRXROXDR)+.  .  . 

RHO/2XLXX2X(XVDRXVOXDR+XWDSXDSXWO+XHDB/2XDBXWO  +  ... 
2*U0x( XDSDSxDS»x2  +  XDBDB/2«DBXX2  +  XDRDRXDR»*2 ) )+  . 
RHO/2XLXX3XXODSNXQOXDSXEPS+RHO/2XLXX2*CXWDSNXWOXDS+. 
2XXD5DSNXUOXDSXX2)XEPS+RHOXLXX2XUOXXPROP+RHO/2XLXX3X 
XQDB/2XDBXQO+RHO/2XLXX2XXMDB/2XDBXWO+RHOXLXX2XUOX. . . 
XDBDB/2XDBXX2 
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FILEs  TLO 


DSL 


A1 


A(l, 2)  =  MASSXRO+RHO/2XLXX3X(XVPXPO+  XVRXRO)  +  RH0/2XLXX2X  ... 
(2KXVVKV0  +  XVDRxUOXDR) 

A(l, 3)  =  -MASSXQO  +  RHO/2XLXX3X(XWQXQO)+RHO/2XLXX2X(2XXWWXWO+ . . . 

XWDSXDSXU0+(XWDB/2XDB+XWDB/2XDB)XU0  +XWDSNXUOXDSXEPS) 
Ad, 4)  =  -MASSXYGXQO-MASSXZGXRO+  RHQ/2XLXX4X(2XXPPXP0+XPR*R0) . . . 
+  RH0/2*LXX3X(XVPXV0) 

Ad, 5)  =  -MASSXM0+2XMASSXXGXQ0  -MASSXYGXPO+RHO/2XLXX4X2XXQQXQO . . 

+RHO/2XLXX3XCXHQXWO+XQDSXDSXUO+XQDB/2XDBXUO)+RHO/'2X  . . . 
LXX3XXQDSNXUOXDSXEPS+RHO/2XLXX3XXQDB/2XDBXUO 
Ad, 6)  =  MASSXVO+2XMASSXXGXRO-MASS*ZGXPO+RHO/2*LXX4X( 2XXRRXR0 . . . 

+  XPRXPO)  +  RHO/2XLXX3XCXVRXVO  +  XRDRXUOxDR) 

Ad, 11)=  -(WEIGHT  -  BOY)XCOS(THETAO) 
x 

A(2, 1 )  =  -MASSXRO+RHO/2XLXX3X(YPXPO+YRXRO)+RHO/2XLXX2X(YVXVO+. . . 
2XYDRXU0XDR) 

A(2,2)  =  RHO/2XLXX3XYVQXQO+RHO/2XLXX2XCYVXUO+YVWXWO) 

A(2, 3)  =  MASSXP0+  RHO/2XLXX3X(YWPXPO+YWRXRO)+RHO/2XLXX2XYVWXVO 
A(2,4)  =  MASSXWO-MASSXXGXQO+2XMASSXYGXPO+RHO/2XLXX4XYPQXQO+. . . 
RHO/2XLXX3X(YPXUO+  YWPXWO) 

A(2,5)  =  -MASSXXGXP0-MASSXZGXR0+RHO/2XLXX4XCYPQXP0+YQRXR0)  +... 
RHO/2*L*X3XYVQXVO 

A(2,6)  =  -MASSXUO+2XMASSXYGXRO-MASSXZGXQO+RHO/2XLXX4XYQRXQO  +... 

RHO/2XLXX3XCYRXUO  +  YWRxWO) 

A(2,10)=  (WEIGHT  -  BOY)xCOS(THETAO)xCOS(PHIO> 

A( 2, 11 ) =  -(WEIGHT  -  BOY)XSIN(THETAO)XSIN(PHIO) 
x 

A(3,l)  =  MASSXQO+RHO/2XLXX3XZQXQO+RHO/2XLXX2X(ZWXWO+2X(JOXZDSXDS. 

+2XUOXZDB/2XDB+(ZWNXWO+2XZDSN*UOXDS)XEPS)+RH0^2XLXX3X. . 
ZQNXQOXEPS+  RHO/2XLXX2X2XUOXZDB/2XDB 
A(3,2)  =  -MASSXPO+RHO/2XLXX3X(ZVPXPO+ZVRXRO)+RHO/2XLXX2X2XZVVXVO 
A( 3, 3)  =  RHO/2XLXX2x(ZWXUO  +  ZWNXUOxEPS) 

A(3,4)  =  -MA$SXVO-MASS*XGXR0+2*MASSXZG*PO+  RHO/'2*L*x4x(2xZPPX.  .  . 

P 0  +  ZPRxRO)  +  RHO/2XLXX3XZVPXVO 
A(3,5)  =  MASSXUO  -  MASSXYGXRO+2XMASSXZGXQO+RHO/2XLXX3XZQXUO  +... 
RH0/2XLXX3XZQNXU0XEPS 

A(3, 6 )  =-MASSXXGXP0-MASSXYGXQ0+RH0^2XLXX4x(ZPRXP0+2XZRRxR0)+. . . 
RHO/2XLXX3XZVRXVD 

A(3,10)=  -(WEIGHT  -  BOY)XCOS(THETAO)XSIN(PHIO) 

A( 3, 11 ) =  -(WEIGHT  -  BOY)XSIN(THETAO)XCOS(PHIO) 
x 

A( 4, 1 )  =  MASSXYGXQO  +  MASSXZGXRO  +  RH0/2XLXX4X( KPxPO  +  ... 

KRXRO )+RHO/2*LXX3X( KVXV0+2*U0X( KDB/2XDB-KDB/2XDB ) )+ . . . 
RHO/2XLXX3XUOXKPROP+  RH0/2XLXX4XKPNXP0XEPS 
A(4, 2)  =  -MASSXYGXPO  +  RH0/2XLXX4XKVQXQ0  +  RH0/2XL XX2x( KVXUO  .  .  . 

+  KVWXWO) 

A(4,3)  =  -MASSXZGXPO  +  RH0/2XLXX4X( KWPxPO  +  KWRxRO)  +  ... 
RH0/2XLXX3XKVWXV0 

A(4,4)  =  -IXYXRO  +  IXZXQO  -  MASSXYGXVO  -  MASSXZGXWO  +  ... 

RHO/2XLXX5XKPQXQO  +  RH0/2xLxx4X( KPXUO+KWPXWO ) 

A(4,5)  =  -IZXRO  +  IYXRO  +  2XIYZXQ0  +  IXZXPO  +  MASSXYGXUO  +... 

RH0/2XLXX5X((CPQXP0  +  KQRXRO )  +  RH0/2XLXX4XKV«XV0 
A(4,6)  =  -IZXQO  +  IYXQO  -  2XIYZXR0  +  MASSxZGxUO  +  ... 

RHO/2XLXX5XKQRXQO  +  RH0/2xLxx4X( KRXUO+KWRXWO ) 

A( 4, 1 0 ) =  -(YGXWEIGHT-YB*BOY)xCOS( THETA 0 )XSIN( PHI 0 ) . . . 

-(ZGXWEIGHT-ZBXBOY)XCOSCTHETAO)XCOS(PHIO) 

A(4, 11 ) =  -(YG*WEIGHT-YBXBOY)XSIN(THETAO)XCOS(PHIO) . . . 
+(ZGXWEIGHT-ZBXBOY)XSIN(THETAO)XSIN(PHIO) 
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X 


X 


X 


X 


X 


AC5 

,1)  = 

A(5i 

,2)  = 

AC5 

,3)  = 

A(5, 

,4)  = 

AC5 

>5)  = 

AC5 

,6)  = 

AC5 

,10)  = 

A(5 

,11)  = 

A(  6 

,1)  = 

A(6 

,2)  = 

AC6 

,3)  = 

A(6 

,4)  = 

A(6 

,5)  = 

A(6 

,6)  = 

A  C  6 

,10)  = 

A(  6 

,11)  = 

A<7 

,1)  = 

A(  7 

,2)  = 

A(7 

,3)  = 

AC  7 

,10)  = 

A(  7 

*-* 

P-* 

w 

II 

A(  7 

II 

(SI 

H 

-MASSXXGXQO  +  RHO/2KLXX4XMQXQO  +  RHO/2xlxx3xMWxwO  +... 
RH0/2*Lxx3xU0XCMDS*DS+MDB''2*DB)  +  RH0/2XLXX4XMQNXQ0X . . . 
EPS  +  RHO/2XLXX3XCMWNXWO  +  2XMDSNXU0XDS)XEPS+ . . . 
RHO/2XLXX3»UO*MDB/2XDB 

MASSXXGXPO  +  MASSXZGXRO  +  RHO/2XLXX4X(MVPXPO  +  . . . 
MVRXRO )  +  RHOXLXX3XMVVXVO 

-MASSXZGXQO  +  RH0/2XLXX3XMWXU0  +  RH0/2XLXX3XMNNXU0XEPS 
-IXXRO  +  IZXRO  -  IYZXQO  -  2XIXZXP0  +  MASSXXGXVO  +  ... 
RH0/2XLXX5X(2XMPPXP0  +  MPRxRO)  +  RH0/2XLXX4XMVPXV0 
IXYXRO  -IYZXPO  -  MASSXXGXUO  -MASSXZGXWO  +  RH0/2X... 
LXX4XMQXU0  +  RH0/2XLXX4XMQNXU0XEPS 

-IXXPO  +  IZXPO  +  IXYXQO  +  2XIXZXR0  +  MASSXZGXVO  +... 

RH0/2XLXX5X(MPRXP0+2XMRRXR0)+RH0/2XLXX4XMVRXV0 

(XGXWEIGHT-XBXBOY)XCOS(THETAO)XSIN(PHIO) 


(XGXWEIGHT-XBXBOY)XSIN(THETAO)XCOSCPHIO)  -  ... 
(ZGXWEIGHT-ZBXBOY)XCOS(THETAO) 

-MASSXXGXRO  +  RHO/2XLXX4XCNPXPO  +NRXRO)  +  RH0/2X... 
LXX3X(NVXVO+2XNDRXUOXDR)+RHOXIXX3XUOXNPROP 
-MASSXYGXRO  +  RHO/2XLXX4XNVQXQO  +  RH0/2XLXX3X( NVXUO+ . . 
NVWXWO) 

MASSXXGXPO  +  MASSXYGXQO  +  RHO/2XLXX4XCNWPXPO+NWRXRO)+ . 
RHO/2XLXX3XNVWXVO 

-IYXQO  +  IXXQO  +  2XIXYXP0  +IYZXRO  +  MASSxXG*HO+ . . . 
RHO/2XLXX5XNPQXQO  +  RHO/2XLXX4X(NPXUO+NWPXWO) 

-IYXPO  +  IXXPO  -  2XIXYXQO  -  IXZXRO  +  MASSXYGXWO+ . . . 
RHO/2XLXX5XCNPQXPO+NQRXRO)  +  RHO/2xixx4XNVQXVO 
IYZXPO  -IXZXQO  -  MASSXXGXUO  -MASSXYGXVO  +  . . . 
RH0/2XLXX5XNQRXQ0  +  RHO/2xLxx4x(NRxUO  +NWRXWO) 
(XGXWEIGHT-XBXBOY)XCOS(THETAO)XCOS(PHIO) 
-(XGXWEIGHT-XBXBOY)XSIN(THETAO)XSINCPHIO)  +. . . 
(YGXWEIGHT-YBXBOY)XCOS(THETAO) 

COS(PSIO)XCOS(THETAO) 

C0SCP5I O)XSIN(THETAO)XSINCPHIO)  -  SIN(PSIO)xCOSCPHIO) 
COSCPSIO)XSINCTHETAO)XCOS(PHIO)  +  SIN(PSIO)xsiN(PHIO) 
VOXCOS(PSIO)XSIN(THETAO)XCOS(PHIO)  +  VOXSIN(PSIO)x. . . 
SINCPHIO)  -  WOXCOS(PSIO)XSINCTHETAO)XSINCPHIO)  +  ... 
WOXSIN(PSIO)XCOSCPHIO) 

-UOXCOS(PSIO)XSINCTHETAO)  +  VOXCOS(PSIO)XCOS(THETAO)X. 
SINCPHIO)  +  WOXCOS(PSIO)XCOS(THETAO)XCOS(PHIO) 
-UOXSIN(PSIO)XCOSCTHETAO)  -  VOXSINC PSIO )xSIN(THETAO )x . 
SINCPHIO)  -  VOXCOSCPSIO)XCOS(PHIO)  -  WOXSIN(PSIO)X . . . 
SIN(THETAO)XSINCPHIO)  +  WOXCOS(PSIO)XSIN(PHIO) 


AC8, 

1)  = 

AC  8 , 

2 )  = 

AC8 , 

3)  = 

ACS, 

.10)  = 

AC8, 

,11)  = 

A  C  8 . 

,12)  = 

A( 

9, 

1)  = 

AC 

9, 

2)  = 

AC 

9, 

3)  = 

AC 

9, 

10)  = 

AC 

9, 

11)  = 

SINCPSIO)xCOS( THETAO) 

SINCPSIO)XSINCTHETAO)XSIN(PHIO)  +  COS(PSIO)xCOS(PHIO) 
SIN(PSIO)XSINCTHETAO)XCOSCPHIO)  -  COS(PSIO)XSIN(PHIO) 
VOXSINCPSIO )XSIN( THETAO )XCOS( PHIO )  -  VOXCOSCPSIO)X. . . 
SINCPHIO)  -  WOXSINCPSIO )x$INC THETAO )XSINC PHIO)  -  ... 
WOXCOSCPSIO)XCOS(PHIO) 

-U0XSIN(PSI0)XSIN(THETA0)  +  VOXSINCPSIO)XCOS(THETAO)X. 
SINCPHIO)  +  HOXSINCPSIO ) xCOS( THETAO )XC0S( PHIO ) 

UOXCOS( PSIO )*COS( THETAO)  +  VOXCOSC PSIO )XSINCTHETAO )X . . 
SINCPHIO)  -  VOXSINCPSIO)XCOS(PHIO)  +  WOxCOSCPSIO)x .  .  . 
SIH(THETAO)XCOSCPHIO)  +  WOxSINCPSIO)xSIN(PHIO) 

-SIN(THETAO) 

COS(THETAO)XSINCPHIO) 

COSCTHETAO)XCOS(PHIO) 

VOXCOS ( THETAO )XC0S( PHI 0)-WOXCOS( THETAO )XSIN( PHIO) 
-U0XC0S(THETA0)-V0XSIN<THETA0)XSIN(PHI0)  -. . . 

WOXSI N ( THETAO )XC0S( PHIO) 
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X 


X 


X 

X 

X 

X 

X 

X 


AdO,  4)  =  1.0 

A(1 0 , 5)  =  SIN(PHI0)xTAN(THETA0) 

A( 10.6 )  =  COS(PHIO)*TAN(THETAO) 

AdO,  10)  =  QOXCOS(PHIO)*TAN(THETAO)  -  ROXSINCPHIO)KTAN(THETAO) 
A( 10/11)=  QOXSIN(PHIO)/COS(THETAO)X1 . 0/C0S( THETAO )  +  ... 
ROXCOS(PHIO)/COS(THETAO)X1 . 0/C0S( THETAO ) 

Adi,  5)  =  COS(PHIO) 

A(ll, 6)  =  -SIN(PHIO) 

A(ll, 10)=  -QOKSIN(PHIO)  -  ROXCOS(PHIO) 

A( 12, 5)  =  SI N( PHI 0 )/COS( THETAO ) 

A( 12,6 )  =  COS ( PHI  0) /COS (THETAO) 

A(12, 10 ) =  Q0KC0S( PHIO)/COS( THETAO )-R0XSIN( PHI 0)/C0S( THETAO) 
A(12, 11 ) =  <JO*SIN(PHIO)/COSCTHETAO)XTAN(THETAO)  +  ... 

ROXCOS ( PHI  0) /COS ( THETAO )XTAN( THETAO) 

MRITEdO,  200  )((A(I»J)/J  =  1»12)»I=1»12) 

CALCULATE  THE  B  MATRIX 


Bd,l)  =  RHO/2*LXX3XXRDR*UOXRO+RHO/2XLXX2X(XRDRXUOXVO+UOXX2*. . 
2XXDRDRXDR ) 

BC1.2)  =  UOXOOXXODB/2  +  UOXWOXXWDB/2  +  U0XX2XXDBDBXDB 

Bd,3)  =  U0XQ0XXQDB/2  +  U0XWQXXWDB/2  +  U0XX2XXDBDBXDB 

Bd,4)  =  UOXQOXXQDS  +  UOXWOXXWDS  +UOXX2X2XXDSDSXDS+RHO/2XLXX3X 
XQDSNXUOXQOXEPS  +  RH0/2XLXX2X(XWDSNXU0XW0  +  2*XDSDSNx 
U0*X2»DS)XEPS 

B( 1 , 5)  =  RHO/2XLXX2X0.12XCDOX0.12XRPM 
Bd,6)  =  SIN(THETAO) 
x 

B(2,l)  =  RHO/2XLXX2XYDRXUOXX2 
B(2,6)  =  -COS ( THETAO )*SIN( PHI  0 ) 
x 

B(3,2)  =  UOXX2XZDB/2XRHO/2XLXX2 

B(3, 3)  =  U0XX2XZDB/2XRH0/2XLXX2 

B(3,4)  =  UOXX2XZDSXRHO/2XLXX2  +  RH0/2XLXX2XZDSNXU0XX2*EPS 
B( 3, 6 )  =  -COS ( THETAO )XCOS( PHI  0) 
x 

BC4,2)  =-RHO/2xLXX3XUOXX2XKDB/2 
B(4, 3)  =  RH0/2XLXX3XU0XX2XKDB/2 

B(4,6)  =  -YBXCOS ( THETAO )XCOS( PHI  0 )  +  ZBXCOSCTHETAO)XSIN(PHIO) 
x 

B( 5, 2 )  =  RHO/2XLXX3XUOXX2XMDB/2 
B( 5, 3 )  =  RHO/2XLXX3XUOXX2XMDB/2 
B( 5, 4 )  =  RHO/2XLXX3X(UOXX2XMDS+MDSNXUOXX2XEPS) 

B(5,6)  =  XBXCOS(THETAO)XCOSCPHIO)  +  ZB*SIN(THETAO) 
x 

B(6 , 1 )  =  RHO/2XLXX3XNDRXUOXX2 

B(6,6)  =  -XB*COS( THETAO )XSIN( PHI 0)  -  YBXSIN(THETAO) 
x 

x  FORMULATE  THE  A  AND  B  MATRIX  FOR  STATE  SPACE  REPRESENTATION 
x 

x  MULTIPLY  MMINV  AND  DF/DX 
x 

DO  80  I  =  1,6 
DO  70  J  =  1,6 
SUM  =0.0 
DO  60  K  =  1,6 

SUM  =  SUM  +  MMINV (I ,K)*A(K,J) 

60  CONTINUE 

AACI,J)  =  SUM 
70  CONTINUE 

SO  CONTINUE 
x 
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X 

x  MULTIPLY  MMINV  AND  DF/DZ 

x 

X 

DO  50  I  =  1,6 

DO  40  J  =  7,12 
SUM  =  0.0 

DO  30  K  =  1,6 

SUM  =  SUM  +  MMINV(  I , K)XA( K, J ) 

30  CONTINUE 

AA( I , J )  =  SUM 
AO  CONTINUE 

50  CONTINUE 

x 
x 

DO  5  I  =  7,12 
DO  6  J  =  1,12 
AA(I.J)  =  AC  I , J  ) 

6  CONTINUE 

5  CONTINUE 

x 
x 

WRITE(10,200)((AA(I,J), J=1 , 12) , I =1 , 12) 
200  FORMAT (  6E12.4) 
x 
x 

*  MULTIPLY  MMINV  AND  DF/DU 

x 

x 


DO  110  I  =  1,6 
DO  100  J  =  1,6 


SUM  =0.0 
DO  90  K  = 

1,6 

SUM  =  SUM 

+  MMINV(I,K)XB(K,J) 

90 

CONTINUE 
BB( I , J )  = 

SUM 

100 

CONTINUE 

110 

X 

CONTINUE 

X 

WRITE(  9,300)((BB(I,J),J=1,6),I=1,6) 

300 

FORMAT (6E12 ■ 4 ) 

x 

x 

DO  405  I  =  1,6 

READ  (2,401 )(GKK(I,J),  J=l,21) 

405  WRITE(3,401  ) (GKK( I ,  J ) ,  J=l,21) 

401  FORMAT ( 3E20 .10) 

x 

x 

ELSE 
END  IF 
x 

x  CALL  ERRSET  (209,256,-1,1,1) 

x  PRINTX,INFO,ISTRAT,IOPT,IONED,IPRINT,IGRAD,NDV,NCON,DX, . . . 

x  OB J , GW, IDG, NGT , IC, DF, AW, NRA, NCOL A , WK, NRWK, . . . 

x  INK , NRIWK 

CALL  DADS( INFO , ISTRAT, IOPT, IONED, IPRINT , IGRAD, NDV, NCON, DX, 
VLB, VUB, OBJ, GW, IDG, NGT , IC, DF, AW, NRA, NCOL A ,WK, NRWK 
IWK, NRIWK) 

IF  (INFO.EQ.  0)  DELPRT=0 . 2 
x 

DERIVATIVE 

NOSORT 

x 

x 

x 
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xxxxxLINEAR  MODELxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx 

X 

X 

X  CALCULATE  BBxu  PART  OF  XDOT  »  AAXX  +  BBXU 
x 

DO  10  J  =  1,6 
SUM  =  0.0 
DO  15  K  =  1,6 

SUM  =  SUM  +  BBC J,K)XUM0DCK) 

15  CONTINUE 

XDOTUCJ)  =  SUM 
10  CONTINUE 
X  CALCULATE  AAXX 

DO  21  J*  1,12 
SUM  =  0.0 
DO  25  K  =  1,12 

SUM  =  SUM  +  AAC J,K)XXCK) 

25  CONTINUE 

XDOTX(J)  =  SUM 
21  CONTINUE 

x  CALCULATE  XDOT  =  AAXX  +  BBXU 
DO  31  J  =  1,6 

XDOTCJ)  =  XDOTX(J)  +  XDOTUCJ) 

31  CONTINUE 

DO  35  J  =  7,12 

XDOTCJ)  =  XDOTXCJ) 

35  CONTINUE 

X 

UDOTM  =  XDOT  Cl) 

VDOTM  =  XD0TC2) 

WDOTM  =  XDOT ( 3 ) 

PDOTM  =  XD0TC4) 


x 

x 

X 


QDOTM  =  XD0TC5) 

RDOTM  =  XD0TC6) 

XDOTM  =  XD0TC7 ) 

YDOTM  =  XD0TC8) 

2D0TM  =  XD0TC9) 

PHMD0T=  XDOT  CIO) 

THETMD=  XDOT  C 11 ) 

PSMDOT=  XDOT  C 12 ) 

WRITEC8,600) 

INTEGRATE  XDOT  TO  GET  THE  STATE  VECTOR  X 


UM  =INTGRL(6 . 0, 
VM=  INTGRL (0.0, 
WM=  INTGRLCO.O, 
PM=  INTGRLCO.O, 
QM=  INTGRLCO.O, 
RM=  INTGRLCO.O, 
XPOSM  =  INTGRL  C 
YPOSM  =  INTGRL C 
ZPOSM  =  INTGRLC 
PHIM  =  INTGRLC  C 
THETAM  =  INTGRL 
PSIM  =  INTGRLC  0 


UDOTM) 

VDOTM) 

WDOTM) 

PDOTM) 

QDOTM) 

RDOTM) 

0.0,  XDOTM) 
0.0,  YDOTM) 
0.0,  ZDOTM) 
.0,  PHMDOT) 
C0.0,  THETMD) 
.0,  PSMDOT) 


x 


*  tt 


X 


X(l)  =  UM 
X( 2)  =  VM 
X( 3 )  =  WM 
XC4)  =  PM 
X(5)  =  QM 
XC6)  =  RM 
X( 7 )  =  XPOSM 
X( 8 )  =  YPOSM 
XC  9 )  =  ZPOSM 
XUO)  =  PHIM 
X(ll)  =  THETAM 
X(12)  =  PSIM 

ZDEPTH  =  ZORD  -  XC9) 

THMANG  =  XC 11 )*57 . 3 
UMODC 1 )=DRM 
UM0DC2)=DBM 
x  DBPM=  UMODC  3) 

UMODC  3 )  =  DBM 
UM0DC4)=DSM 
UMODC  5 ) =DRPM 
UMODC6 ) =DBOY 
x 

PHANG=PHIM/0 .0174532925 
THMANG=THETAM/0 . 0174532925 
PSMANG=  PSIM/  0.0174532925 
PITCHM=THMANG 
DEPTH=-ZPOSM 


X 

XXXXXXCONTROL  LAWXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX 

X 

X 

x  DBS  =  UM0DC2) 

x  DBP  *  UMODC  3 ) 

*  DS  =  UMODC 4) 

x  DR  =  UMODC 1) 

x 
X 

x  PUT  IN  STERN  AND  BOW  PLANE  STOPS 

x 

x  IFCABSCDBS) .GT.0.6)  THEN 

x  DBS  =  0 . 6XABSC  DBS)/DBS 

x  ENDIF 

x  IFCABSCDBP). GT.0.6)  THEN 

x  DBP  =  0 ,6XABSCDBP)/DBP 
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x  ENDIF 

x  IFCAB5CDS).GT.0.6)  THEN 
x  DS  =  0 . 6*ABS( DS )/DS 

x  ENDIF 

x 

INTGRD  =  (UMXUM+VMXVM+WMXWM+PMXPM+QMXQM+RMXRM+. . . 

XPOSMxXPOSM+CYPOSM-YORD)XCYPOSM-YORD) . . . 
+CZPOSM-ORDDEP )XCZPOSM-ORDDEP )+PHIMxPHIM+ . . . 
THETAMXTHETAM+PSIMXPSIM)  +  < DSMXDSM+DBMXDBM+DRMXDRM) 
0BJ1  =  INTGRL ( 0 . , C  0 . 5  JXINTGRD) 

OBJ  =  OBJ1 


DYNAMIC 

RN=TIME/(FINTIM/10.-DELT/10000. ) 

PN=TIME/(FINTIM/20 .-DELT/10000 . ) 

0=INT ( RN)+1 
P=INT(PN)+1 

IFCO. GE.il)  G=10 

IFCP. GE.20)  P=20 
x 

X  ADDITIONALLY  THE  PLANES  SHOULD  BE  AT  EQUILIBRIUM  SO  THE 
x  VEHICLE  WILL  PROCEED  AT  THIS  NEW  DEPTH  WITHIN  SOME  TOLERANCE 
x 

D5M=DX( 0) 

DBM=DX( 10+0) 

IFCO.GE.IO)  DSM-0 . 000 
IFCO.GE.IO)  DBM=0 .000 
DRM=DXC20+P ) 

X  RPM=DXC  30+0) 

X 

x  CONSTRAINTS  FOR  A  DIVE 

x 

x  ORDERED  DEPTH  =  ORDDEP 

GWC1)  =  (ZPOSM-ORDDEP)X . 5 
GWC2)  =  ( ORDDEP-ZPOSM) X . 5 

x  AUV'S  FINAL  STATE  MUST  BE  LF.VEL  FLIGHT  AS  FOLLOWS 
GWC3)  =  THETAM 
GW( A )  =  -THETAM 
GWC  5)  =  PHIM 
GWC  6 )  =  -PHIM 
GW  C  7 )  =  PSIM 
GWC  8 )  =  -PSIM 
GW(9)=(YP0SM-Y0RD)/.A 
GWC 10 ) =  C YORD-YPOSM)/ . A 
GWC 11)  =  -ZPOSM 
x 

x  AVOIDING  THE  OBSTACLE 
x 

x  DIST1 =SQRT C  CXPQSM-X0BS1 )XC  XP0SM-XQBS1 )+  .  .  . 

x  CZP0SM-Z0BS1 )XCZPOSM-ZOBS1 ) ) 

x  IF  CDIST1 .LT.DSAVE1)  DSAVE1=DIST1 

x  GWC 6 )  =  C1.-DSAVE1) 

NDX=XP05M/17 .  A25 
NDZ=ZP0SM/17 . A25 
NDT=TIME*6 ./17 . A25 
TERMINAL 

x  WRITECll ,66 )  XPOSM,YPOSM, DEPTH 

X66  FORMAT  C IX, F10 . 3, F10 . 3) 

IFCINFO.EQ.O)  THEN 
PRINTx , 0 , P 
9000  CONTINUE 
ELSE 
ENDIF 

IFCINFO.EQ.O)  CALL  ENDJOB 
CALL  RERUN 
x 

END 

STOP 


BD 


FILE:  TNLO 


DSL 


A1 


TITLE  RUN: 16-5  NONLINEAR  AUV  MODEL  /  STERN  PLANE  AND  BOW  PLANE  SEPARATED 

*  (1)  UPDATED: 05/20/88 

*  (2)  RIGHT  OBJ  EQUATION 

X  (3)  ADS  CONSTRAINTS  ON  DEPTH, PITCH, YAW, ROLL  AND  Y  POSITION 
x  (4)  CORRECTED  OBSTACLE  AVOIDANCE  ROUTINE 

XXXXXXXXXXXXXXXXXXXXXADSL  SET-UPxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx 
FIXED  ISTRAT,  IOPT,  IONED,  IPRINT,  INFO,  IGRAD,  NDV,  NCON 
FIXED  IDG,  NGT,  IC,  NRA,  NCOLA,  NRWK,  IWK,  NRIWK,  0,  H,D,C,PP 
D  DIMENSION  AWC42.42) 

ARRAY  WKC5000),  IWKC1000) 

ARRAY  DX( 40 ) ,  VLB( 40 ) ,  VUBC40),  GW(ll),  DFC41),  IDGC11),  IC(ll) 

PARAM  NRA=42,  NC0LA=42,  NRWK=5000,  NRIWK=1000 
PARAM  IGRAD=0,  INFO=0,  NDV=40,  NCON=ll,  NGT=11 
TABLE  DXC1-2)=2*.0,DXC3-40)=38X0.,  IDGCl-10)=10x-l 
TABLE  IDG(11)=1X1 

TABLE  VLB(1-09)=09X-. 17452,  VLBC 11-1 9 )=09x- . 2443 , VLBC20 ) =0 . , VLBC 1 0 ) =0 . 
TABLE  VUBC 1-09 )=09X. 17452,  VUBC 1 1 -1 9 ) =09x . 2443 . VUBC 20 ) =0 . , VUB( 1 0 ) =0 . 

TABLE  VLBC 21-39 )=19x-. 62367, VUBC 21-39 )=19x. 623627, VUB( 40-41) =2*0. 

TABLE  VLBC  40-41 )  =  2*0 . 

PARAM  ISTRAT=3 ,  IOPT=l,  IONED=l,  IPRINT=0000 
INCON  H=0,  OBS1=0 . , YZ0NE=0 . 

METHOD  RECT 

CONTROL  FINTIM=21.0,DELT=.10 

PRINT  XP0S,YP0S,ZP0S, PITCH, THEANG, DR, DS 

XRINT  THETAD.W, DEPTH, PITCH, XPOS , DEPTH, NDX, NDZ, NDT 

XRINT  DS, DB, DR, DEPTH, PITCH, XPOS,  YPOS,  ZPOS,  NDT 

XAVE  THETA, W,Z, DEPTH, PITCH, DS, DB, BONANG, STNANG 

XRAPH(DE=TEK618)  TIME, DS 

XRAPHC  DE=TEK6 18 )  TIME, DEPTH 

XRAPHC  DE=TEK6 18 )  TIME, WDOT 

XRAPHC DE=TEK6 18 )  TIME,W 

XRAPHC DE=TEK618)  TIME, THETDD 

XRAPHC  DE=TEK6 18 )  TIME, THETAD 

XRAPHC  DE=TEK6 18 )  TIME, THETA 

XRAPHCDE-TEK618)  TIME, PITCH 

XRAPHC  DE=TEK618 )  TIME, BOWANG 

XRAPHC  DE=TEK618 )  TIME, STNANG 

xxxxxxxxxxxxxxxxxxxxxxxxxxxxDSL  MODEL  SET  UPxxxxxxxxxxxxxxxxxxxxxxxxxxx 
x 

D  DIMENSION  MMC6 , 6 ) , G4C 4 ) , GK4C 4 ) , BRC 4 ) , HHC 4 ) 

D  DIMENSION  BC 6 , 6 ) , BBC  6 , 6 ) 

D  DIMENSION  AC12.12),  AAC12,12) 

D  COMMON  /  BLOCK1  /  FC12),  FPC6 ),  MMINVC6,6),  UCFC4) 

FIXED  N, I A . IDGT ,IER,LAST,J,K,M,JJ,KK,I 
INTEGER 

ARRAY  WKAREAC54) ,  XC 12 ) 

x 

x 

CONST 

X 

x  LONGITUDINAL  HYDRODYNAMIC  COEFFICIENTS 
x 

CONST  XPP  =  , XQQ  =  , XRR  =  ,XPR  =  ,... 

XUDOT  =  , XWQ  =  ,XVP  =  , XVR  =  ,... 

XQDS=  , XQDB=  , XRDR=  ,XVV=  ,... 

XWW  =  , XVDR=  , XWDS=  ,XWDB= 

XDSDS=  , XDBDB=  , XDRDR=  ,XQDSN=  ,... 

XHDSN=  , XDSDSN= 

x 
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X 


* 

CONST 


x 

X 


X 

CONST 


LATERAL  HYDRODYNAMIC  COEFFICIENTS 


YPDOT  = 
YVDOT  = 
YWP  = 
YDR  = 


,YRDOT= 
>  YP  = 
,YWR  = 
t  CDY  = 


» YPQ  = 
,YR  = 

>  YV  = 


» YQR  = 
i  YVQ  = 
» YVW  = 


NORMAL  HYDRODYNAMIC  COEFFICIENTS 


ZQDOT® 
ZWDOT = 
ZW  = 
ZQN  = 


ZPP  = 

» ZPR  = 

,  ZRR 

ZQ  = 

,ZVP  = 

,  ZVR 

ZW  * 

» ZDS  = 

,ZDB 

ZWN  = 

» ZDSN= 

>  CDZ 

x 

*  ROLL  HYDRODYNAMIC  COEFFITIENTS 
x 


CONST 

KPDOT® 

,  KRD0T= 

» KPQ  = 

,KQR  = 

KVDOT  = 

,  KP  = 

,  KR  = 

i  KVQ* 

KWP  = 

,  KWR  = 

,KV  = 

,KVW  = 

¥ 

KPN  = 

,  KDB  = 

X 

PITCH 

HYDRODYNAMIC  COEFFICIENTS 

CONST 

MQDOT  = 

,  MPP  = 

,MPR  = 

» MRR  = 

MHDOT= 

,  MQ  = 

,  MVP  = 

,  MVR  = 

MW  = 

,  MW  = 

,MDS  = 

,  MDB  = 

MQN  = 

,  MWN  = 

, MDSN  = 

X 

X 

YAW  HYDRODYNAMIC  COEFFICIENTS 

CONST 

NPDOT= 

,  NRDQT= 

t  NPQ  = 

,  NQR  = 

NVDOT  * 

,  NP  « 

,  NR  = 

,NVQ  = 

NWP  = 

,  NWR  = 

,NV  = 

,NVW  = 

NDR  = 

X 

£ 

MASS  CHARACTERISTICS  OF  THE  FLOODED  MARK 

IX  VEHICLE 

CONST 

WEIGHT 

=  ,  BOY  = 

,  VOL  = 

,XG  = 

YG  = 

,  ZG  = 

,  XB  = 

•  ZB  = 

IX  = 

,  IY  = 

,IZ  = 

,IXZ  = 

IYZ  = 

,  IXY  = 

,YB  = 

L  = 

,  RHO  = 

>  G  = 

,  NU  = 

AO  = 

, KPROP  = 

/ NPROP  = 

/ 

X1TEST 

DEGRUD=  0.0  ,DEGSTN=  0.0 


x 

x 

XONST  X0BS1=36 . 0 
XONST  Z0BS1  =  -12 . 0 
x 

x  INPUT  INITIAL  CONDITIONS  HERE  IF  REQUIRED 
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*  *  *  *  *  *  *  *  * 


INITIAL 

x  DSAVE1 =SQRT ( (XP0S-X0BS1 )X(XPOS-XOBSl )+(ZPOS-ZOBSl )*(ZP0S-ZQBS1 ) ) 

NOSORT 

ORDDEP  =  17.425 
*  Y0RD=40 . 0 

D=0 
H=H+1 

IF(H.EQ.l)  THEN 
U  =  0.0 
V  =  0.0 
W  =  0.0 
P  =  0.0 
Q  =  0.0 
R  =  0.0 
XPOS  =0.0 
YPOS  =0.0 
ZPOS  =0.0 
PSI  =  0.0 
THETA  =  0.0 
PHI  =  0.0 
x 

UO  =  6.0 
VO  =  0.0 
WO  =  0.0 
PO  =  0.0 
<30  =  0.0 
RO  =  0.0 
PHIO  =  0.0 
THETAO  =0.0 
PSIO  =  0.0 
DB=  0.0 
DS  =  0.0 
DR  =  0.0 
RPM  =  500 

LATYAW  =0.0 
NORPIT  =0.0 
RE  =  UOXL/NU 

CDO  =  .00385  +  (1 .296E-17 )X(RE  -  1.2E7)**2 

DEFINE  LENGTH  FRACTIONS  FOR  GAUSS  QUADUTURE  TERMS 

G4( 1 )  =  0.069431844 
G4( 2 )  =  0.330009478 
G4(3)  =  0.669990521 
G4( 4)  =  0.930568155 

DEFINE  WEIGHT  FRACTIONS  FOR  GAUSS  QUADUTURE  TERMS 

GK4( 1 )  =  0.1739274225687 
GK4( 2 )  =  0.3260725774312 
GK4( 3)  =  Q. 3260725774312 
GK4( 4 )  =  0.1739274225687 

DEFINE  THE  BREADTH  BB  AND  HEIGHT  HH  TERMS  FOR  THE  INTEGRATION 

BR(1)  =  75.7/12 
BRC2)  =  75.7/12 
BRC3)  =  75.7/12 
BR(4)  =  55.08/12 
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*  * 


HHC 1 )  =  16.38/12 
HH(2)  =  31.85/12 
HHC  3)  =  31.85/12 
HHC4)  =  23.76/12 
x 

MASS  =  HEIGHT/G 

x 

DIVAMP  =  DEGSTNKO. 0174532925 
RUDAMP  =  DEGRUDXO. 0174532925 


10 

15 

x 

x 

x 

x 

X 

X 


X 


X 


X 


N  =  6 

DO  15  J  =  1 » N 
DO  10  K  =  1 , N 
MMINVC J,K)  =  O.C 
MM  C  J , K )  =  0.0 
CONTINUE 
CONTINUE 


MMCl.l)  =  MASS  -( ( RHO/2 )XCLXX3) xXUDOT ) 
MMC 1,5)  =  MASSX2G 
MMC 1.6)  =  -MASSXYG 


MMC2,2) 
MMC2,4) 
MMC 2/ 6 ) 


MASS  -C  C  RHO/2 )#C 1**3) XYVDOT ) 
-MASSXZG  -( C RHO/2 )*CL**4)*YPD0T) 
MASSXXG  -  C(RH0/2)*(L**4)XYRD0T) 


MMC  3,3)  =  MASS  -  C (RH0/2)*CL*X3)*ZWD0T) 

MMC 3, 4)  =  MASSXYG 

MMC 3# 5)  =  -MASSXXG  -( CRH0/2)X(LXX4)XZQD0T) 


MMC4,2)  =  -MASSXZG  -  C CRH0/2)X(LXX4)XKVD0T) 
MMC4.3)  =  MASSXYG 

MMC 4,4)  =  IX  -  (CRH0/2)X(IXX5)XKPD0T) 

MMC 4, 5)  =  -IXY 

MMC 4, 6 1  =  -IXZ  -<CRH0/2)X<LX*5)*KRD0T) 


MMC 5,1)  =  MASSXZG 

MMC  5, 3)  =  -MASSXXG  -<  C  RHO/2 )XC  LXX4 )*MWDOT ) 
MMC 5, 4)  =  -IXY 

MMC 5, 5)  =  IY  -<( RHO/2 )X(LXX5)XMQD0T) 


x 


x 


MMC5, 6 )  =  -IYZ 


MMC6 , 1 ) 
MMC  6,2) 
MMC  6 , 4  ) 
MMC6 , 5 ) 
MMC6 , 6 ) 


-MASSXYG 

MASSXXG  -<<RH0/2)*CLXX4)XNVD0T) 
-IXZ  -  C CRH0/2)XCLXX5)XNPD0T) 
-IYZ 

IZ  -  <  C RHO/2 ) XC  L**5 )*NRD0T) 


7D 


! 


t 


1 


t 


X 

LAST  =  NXN+3XN 
DO  20  M  =  1 , LAST 
WKAREACM)  =0.0 
20  CONTINUE 
x 

IER  =  0 
IA  =  6 
IDGT  =  4 

WRITEC  8 ,  400  ) ( ( MMC I ,  J  )  ,  J  =  1,6), I  =  1,6) 

X 

CALL  LINV2F(MM,N,IA,MMINV, IDGT, WKAREA, IER) 

X 

WRITEC  8,400)((MMINV(I, J),  J  =  1,6), I  =  1,6) 

400  F0RMATC6E12.4) 
x 

ELSE 

ENDIF 

x 

x 

CALL  DADS ( INFO , ISTRAT , IOPT, IONED, IPRINT, IGRAD, NDV, NCON, DX, . . . 

VL  B , VUB, OBJ , GW, IDG, NGT , IC, DF, AW, NRA, NCOLA, WK, NRWK, . . . 
IWK.NRIWK) 

C  IFCH.EQ.l)  THEN 
C  WKC12)  =  .002 

C  CALL  DADSCINFO, ISTRAT, IOPT, IONED, IPRINT, IGRAD, NDV, NCON, DX, . . . 

C  VLB, VUB, OBJ, GW, IDG, NGT, IC,DF, AW, NRA, NCOLA, WK, NRWK, . . . 

C  IWK.NRIWK) 

IFCINFO.EQ.O)  DELPRT  =  0.2 
X  IFCINFO.EQ.O)  DELPLT  =  0.2 

x 
x 
x 

DERIVATIVE 

NOSORT 

x 

x 

x  PROPULSION  MODEL 

x 

X 

SIGNU  =1.0 

IF  CU.LT.0.0)  SIGNU  =  -1.0 
IF  CABSCU) .LT. XI TEST )  U  =  X1TEST 
SIGNN  =1.0 

IF  CRPM.LT.0.0)  SIGNN  =  -1.0 
ETA  =  0.012XRPM/U 
RE  =  UXL/NU 

CDO  =  .00385  +  Cl ,296E-I7)XCRE  -  1.2E7)XX2 

CT  =  0. 008XLXX2XETAXABSCETA)/CAO) 

CT1  =  0 . 008XLXX2/CA0) 

EPS  =  -1  .  O+SIGNN/SIGNUXC SQRT CCT+1.0)-1.0)/(SQRTCCT1  +  1.0)-1.0) 
XPROP  =  CDOxC  ETAXABSC  ETA )  -  1.0) 
x 
x 
x 

x  CALCULATE  THE  DRAG  FORCE,  INTEGRATE  THE  DRAG  OVER  THE  VEHICLE 
x  INTEGRATE  USING  A  4  TERM  GAUSS  QUADUTURE 
x 

X 

LATYAW  =0.0 
NORPIT  =  0.0 
DO  500  K  =  1,4 

UCFCK)  =  SQRTCCV+G4CK)XRXL)XX2  4  CW-G4CK)XQXL )XX2) 

IFCUCFCK) .GT.1E-10)  THEN 


* 
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XXX  XXX  X  X  X 


FILE-.  TNLO 


A1 


DSL 


500 

* 

x 

x 

x 

x 

x 

X 


TERMO 

TERM1 
TERM2 
LATYAW 
NORPIT 
END  IF 
CONTINUE 


(RH0/2)#(CDY*HH(K)x( V+G4(K)XRXL)XX2  +. . . 

CDZXBR(K)*(W-G4(K)XQXL)XX2) 

TERM0X(V+G4(K)XRxL)/UCFCK) 

TERMOX(W-G4CK)XQXL)/UCF(IO 

LATYAW  +  TERM1XGK4UOXL 

NORPIT  +  TERM2*GK4( K)*L 


FORCE  EQUATIONS 


LONGITUDINAL  FORCE 

FP Cl)  =  MASSXVXR  -  MASSXWXQ  +  MASSXXGXQXX2  +  MASSXXGXRXX2- . . . 

MASSXYGXPXQ  -  MASSXZGxPxR  +  (RHO/2>xlxx4x(XPPxPxx2  +... 
XQQXQXX2  +  XRRXRXX2  +  XPRxpxR)  +(RH0/2)XLXX3X(XWQXWXQ  + . . . 
XVPXVXP+XVRXVXR+UXQX(XQDSXDS+XQDBXDB)+XRDRXUXRXDR)+ . . . 
(RH0/2)XLX*2*(XVVXVXX2  +  XWWXWXX2  +  XVDR*U*VXDR  +  UXWx . . . 

( XWDS*DS+XWDB*DB )  +  Uxx2x( XDSDSxDSxx2+XDBDB*DB**2+  •  ■  ■ 

XDRDRXDRXX2) )-( HEIGHT  -BOY)XSIN(THETA)  +CRH0/2)XLXX3X  ... 
XQDSNXUXQXDSXEPS+(RH0/2)XLXX2X(XWDSNXUXWXDS+XDSDSN*UXX2*. . 
DSXX2 ) XEPS  +(RH0/2)XLXX2XUXX2XXPR0P 


LATERAL  FORCE 


FP(2)  =  -MASSXUXR  -  MASSXXGXPXQ  +  MASSXYGXRXX2  -  MASS*ZGXQXR  +... 

(RH0/2)*L*K4X(YPQ*PXQ  +  YQRXQXR)+(RH0/2)XLXX3X(YPXUXP  +... 
YRXUXR  +  YVQXVXQ  +  YWPXWXP  +  YWRXWXR)  +  ( RHO/2 ) XL **2*  ... 

(YVXUXV  +  YVWXVXW  +YDRXUXX2XDR)  -LATYAW  +(WEIGHT-BOY)x . . . 
COS(THETA)XSINCPHI) 

NORMAL  FORCE 

FP(3)  =  MASSXUXQ  -  MASSXVXP  -  MASSXXGXPXR  -  MASSxYGxQxR  + 

MASSXZGXPXX2  +  MASSXZGXQXX2  +  (RH0/2)XLXX4X(ZPPXPXX2  +... 
ZPRXPXR  +  ZRRXRXX2)  +  ( RHO/2 ) XL xx3x( ZQXUXQ  +  ZVPXVXP  +... 
ZVRXVXR)  +(RH0/2)xlxx2x(ZWxUxw  +  ZVVXVXX2  +  Uxx2x( ZDSx . . . 
DS+ZDBXDB)  )-NORPIT+(WEIGHT-BOY)xC.OS(THETA)XCOS(PHI )+ .  .  . 

C RHO/2 ) XL XX3XZQNXUXQXEPS  +(RH0/2)xlxxzx(ZWNxuxw  +ZDSNX  .  .  . 
Uxx2xDS)XEPS 

ROLL  FORCE 

FP( 4 )  =  -IZXQXR  +IYXQXR  -IXYXPXR  +IYZXQXX2  -I YZ*Rxx2  +IXZXPXQ  +.. 

MASSXYGXUXQ  -MASSXYGXVXP  -MASSxZGxwxp+( RHO/2 ) xlxxsxc KPQx . . 
PXQ  +  KQRXQXR)  +( RHO/2 )XLXX4X(KPXUXP  +KRXUXR  +  KVQXVXQ  +.. 
KWPXWXP  +  KWRXWXR)  +(RH0/2)XLXX3X(KVXUXV  +  KVWXVXW)  +  ... 

(YGXWEIGHT  -  YBXB0Y)XC0S(THETA)XC0S(PHI )  -  (ZGXWEIGHT  -  .. 
ZBXB0Y)XC0S(THETA)XSINCPHI )  +  (RHQ/2)XLXX4*KPNXUXPXEPS+  .. 
( RHO/2) XL XX3XUXX2XKPROP  +MASSXZGXUXR 
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*  *  *  *  *  *  *  *  *  *  *  *  *  ^  ******  *  *  ***  ** 


PITCH  FORCE 

FP(5)  =  -IXXPXR  +IZ*PXR  +IXYXQXR  -IYZXPXQ  -IXZXPXX2  +IXZXRXX2 

MASSXXGXUXQ  +  MASSXXGXVXP  +  MASSxZGxvxR  -  MASSXZGXWXQ  +.. 
(RH0/2)XLXX5X(MPPxpxx2  +MPRXPXR  +MRRxRxx2)+CRH0/2)xLxx4x. 
( MQXUXQ  +  MVPXVXP  +  MVRxVxR)  +  (RH0/2)XLXX3X(MWXUXW  +  ... 
MVVXVXX2+UXX2X(MDSXDS+MDBXDB))+  NORPIT  -(XGXWEIGHT-  ... 
XBXBOY)XCOSCTHETA)XCOS(PHI)  +  ... 

(RH0/2)XLXX4XMQNXUXQXEPS  +... 

(RH0/2)XLXX3X(MWNXUXW+MDSNXUXX2XDS)XEPS- 
(ZGXWEIGHT-ZBXBOY)XSIN(THETA) 

YAW  FORCE 

FP(6 )  =  -IYXPXQ  +IXXPXQ  +IXYXPXX2  -IXYXQXX2  +IYZXPXR  -IXZXQxR  -. 

MASSXXGXUXR  +  MASSXXGXWXP  -  MASSxYGxvxR  +  MASSXYGXWXQ  +.. 
CRH0/2)XLXX5X(NPQXPXQ  +  NQRXQXR )  +CRH0/2)XLXX4X(NPXUXP+ .  . 
NRXUXR  +  HVQXVXQ  +NHPXWXP  +  NWRxwxR)  +(RH0/2)xlxx3X(HVX. . 
UXV  +  NVWXVXW  +  NDRXUXX2XDR)  -  LATYAW  +  (XGxWEIGHT  -  ... 

XBXB0Y)XC0S( THETA )XSIN( PHI )  +  (YGxWElGHT)xsiN( THETA) .  .  . 
+<RHQ/2) XL XX3XUXX2XNPROP-YBXBOYXSIH( THETA) 

IFCZ . EQ . 50 . 0)THEN 

WRITE  <S,500)C FP( I ) ,  I  =  1,6) 

Z  =  0.0 
END  IF 

NOW  COMPUTE  THE  F(l-6)  FUNCTIONS 

DO  600  J  =  1,6 

F(J)  =  0.0 
DO  600  K  =  1,6 

F(J)  =  MMINVCJ,K)XFP(K)  +  F(J) 

CONTINUE 

THE  LAST  SIX  EQUATIONS  COME  FROM  THE  KINEMATIC  RELATIONS 

FIRST  SET  THE  DRIFT  CURRENT  VALUES 

UCO  =  0.0 
VCO  =0.0 
WCO  =0.0 

INERTIAL  POSITION  RATES  F(7-9) 

FC7)  =  UCO  +  UXCOS(PSI)XCOSCTHETA)  +  VX(COS(PSI )*SIN(THETA)X . . . 

SIN (PHI)  -  SINCPSI)XCOS(PHI))  +  WX( COSC PSI ) XSINC THETA )x . . 
COS(PHI)  +  SIN( PSI) XSINC PHI)) 

F(8)  =  VCO  +  UXSIN( PSI )XCOS( THETA)  +  VX( SIN( PSI )XSIN( THETA )x .. . 

SIN(PHI)  +  COSCPSDXCOSCPHI )  )  +  WX(SIN(PSI  )XSIN(THETA)X.  . 
COSC  PHI  )  -  COS(PSI)XSINCPHD) 

FC 9 )  =  WCO  -  UXSINCTHETA)  +VXCOSCTHETA )xSIN( PHI )  +WXCOS(THETA)x . 
COSC  PHI ) 

EULER  ANGLE  RATES  FC 10-12) 

FC  10 )  =  P  +  QXSIN(PHI)XTANCTHETA)  +  RxCOSC  PHI )xTAN( THETA ) 

FC  11 )  =  QXCOSC  PHI )  -  RXSINCPHI) 

FCI2)  =  QXSINCPHD/COSCTHETA)  +  RXCOSCPHI )/COS(THETA) 


x 


*  X 


X 

x 

K  IF  (Z.EQ. 1.0) WRITE  (9, 500) ( F( I ) ,  I  =  1,12) 

*00  FORMAT (6E12 . 9) 

x  Z  =  Z  +  1 

x 
x 

UDOT  =  FU) 

VDOT  =  F(2) 

WDOT  =  F(3) 

PLOT  =  F( 4) 

QDOT  =  F(  5) 

ROOT  =  FC  6 ) 

XDOT  =  F ( 7  ) 

YDOT  =  FC8) 

ZDOT  =  F( 9  ) 

PHIDOT  =  FC 1 0 ) 

THETAD  =  FC 11 ) 

PSIDOT  =  FC 12) 
x 

U  =  INTGRL  ( UO , UDOT ) 

X  X(l)  =  U 

V  =  INTGRL (0.0 , VDOT ) 
x  X(  2 )  =  V 

W  =  INTGRL ( 0 . 0 , WDOT  ) 
x  X(  3)  =  W 

P  =  INTGRL ( 0 . 0 , PDOT ) 

x  X( 4 )  =  P 

Q  =  INTGRL (0.0, QDOT) 
x  X( 5)  =  Q 

R  =  INTGRL ( 0 . 0, RDOT) 

X  X( 6 )  =  R 

XP05  =  INTGRL ( 0 . 0, XDOT ) 

X  X(7 )  =  XPOS 

YPOS  =  INTGRL ( 0 . 0, YDOT) 

X  X(8 )  =  YPOS 

Z  =  INTGRL (0.0, ZDOT) 
x  X( 9 )  =  ZPOS 

PHI  =  INTGRL(0.0, PHIDOT) 
x  X(1C)  =  PHI 

THETA  =  INTGRL(0.0, THETAD) 

X  X(ll)  =  THETA 

PSI  =  INTGRUO.O, PSIDOT) 

X( 12 )  =  PSI 

PHIANG  =  PHI/0.0174532925 
THEANG  =  THETA/0.0174532925 
PSIANG  =  PSI/O, 0174532925 
ZPOS=-Z 
X 

DEPTH=ZPOS 
PITCH=THEANG 
BOWANG=(DB/. 01745) 

5TNANG=(DS/. 01745) 

INTGRD  =  (UXU+VXV+WXW+PXP+QXQ+RXR+XP0SXXP0S+(YP0S-Y0RD)X, . 
(YP0S-Y0RD)+(Z-0RDDEP)X(Z-0RDDEP)+PHIXPHI+ . . . 
THETAXTHETA+PSIXPSI)  +  (DSXDS+DBXDB)+(DRXDR) 

OBJ  1  =  INTGRL(0.,(0.5)XINTGRD) 

OBJ  =  0BJ1 
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******  *  *****  **** 


DYNAMIC 

RN=TIME/(FINTIM/10. -DEL T/ 10000. ) 

PN=TIM£/(FINTIM/20 .-DELT/ 10000 . 1 
0=INT(RN)+1 
PP=INT(PN)+1 
IFC0.GE.10)  0=10 
IF( PP . GE . 20 )  PP=20 

ADDITIONALLY  THE  PLANES  SHOULD  BE  AT  EQUILIBRIUM  SO  THE 
VEHICLE  WILL  PROCEED  AT  THIS  NEW  DEPTH  WITHIN  SOME  TOLERANCE 

DS=DX<  0 ) 

DB=DX(10+0) 

IF(O.GE.IO)  DS=0 . 

IF(O.GE.IO)  DB=0 . 

DR=DX( 20+PP 1 
RPM=DX( 30+0) 

CONSTRAINTS  FOR  A  DIVE 

ORDERED  DEPTH  =  ORDDEP 
GW(1)  =  (Z-0RDDEP)*.5 
GWC 2 )  =  ( ORDDEP-Z )* . 5 

AUV'S  FINAL  STATE  MUST  BE  LEVEL  FLIGHT  AS  FOLLOWS 
GW( 3 )  =  THETA 
GWC4)  =  -THETA 
GWC5)=  C YPOS-YORD)/ . 4 
GWC  6 ) =  ( YORD-YPOS)/ . A 
GW( 7 ) =PSI 
GWC  8  1 =-PSI 
GW( 9 ) =PHI 
GWC 1 0 ) =-PHI 
GWC 11 ) =ZPOS 

AVOIDING  THE  OBSTACLE 

DIST1 =SQRT ( CXP0S-X0BS1 )*C  XP0S-X0BS1 l+CZPOS-ZOBSl 1XCZP05-Z0BS1 ) ) 
IF  ( DIST1 . LT . DSAVE1 1  DSAVE1=DIST1 
GW(6 )  =  C1.-DSAVE1) 

NDX=XP0S'17.425 
NDZ=ZP0S/17 .425 
NDT  =TIME*6 . /17 . 425 
TERMINAL 

IFCINFO.EQ.Ol  THEN 

*  PRINT*, DSAVE1 
*9999  FORMAT C1X,E15.4) 

9000  CONTINUE 
ELSE 
ENDIF 

IFCINFO.EQ.Ol  CALL  ENDJOB 
CALL  RERUN 

* 

END 

STOP 
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FILE:  TX 


DSL 


A1 


TITLE  LINEAR  AUV  MODEL  /  STERN  PLANE  AND  BOW  PLANE  SEPARATED 

TITLE  WITH  COMMANDS  TO  NONLINEAR  MODEL 

XU)  UPDATED:  05/30/88 

X  (3)  RIGHT  OBJ  EQUATION 

x  (4)  ADS  CONSTRAINTS  ON  DEPTH  AND  PITCH 

x  (5)  OBSTACLE  FURTHER  DOWN  THE  TRAJECTORY  AND  ABOVE  IT 

x  (6)  CORRECT  OBSTACLE  AVOIDANCE  ROUTINE  ADDED 

xxxxxxxxxxkxkxxxxxxxxADSL  SET-UPxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx 
FIXED  ISTRAT,  IOPT,  IONED,  IPRINT,  INFO,  IGRAD,  NDV ,  NCON 
FIXED  IDG,  NGT ,  IC,  NRA,  NCOLA,  NRWK,  IWIC,  NRIWK,  0,  H,D,C,PP 
D  DIMENSION  AWC42,42) 

ARRAY  WKC4000  ) ,  IWKC1000) 

ARRAY  DXL40),  VLBC 40 ) ,  VUB(40),  GW(07),  DF(41),  IDGC07),  IC(07) 

PARAM  NRA=42,  NC0LA=42,  NRWK=4000,  NRIWK=1000 
PARAM  IGRAD=0 ,  INFO=0,  NDV=40,  NCON=07,  NGT=07 
TABLE  DX(1-2)=2X.0,DX(3-21)=19X0. ,  IDGC 1-6 ) =6X-1 
TABLE  DXC  22-40 )  =  19XQ . 

TABLE  IDGC  7-0 )  =  lxl 

TABLE  VLBC 1-9 )=9X-. 17452,  VLBC 1 1 -1 9 ) =9x- . 2443, VLBC1 0 ) =0 . ,VLBC20)=0. 

TABLE  VUB(1-9)=9X. 17452,  VUBC1 1 -1 9 ) =9x . 2443 , VUBC1 0 ) =0 . ,VUBC20)=0. 

TABLE  VL BC 21 -39 )=19x-. 523627 ,VUBC 21-39 )  =  19x. 523627, VUBC 40-41)  =2X0 . 

TABLE  VLBC40-41  )=2*0  . 

PARAM  ISTRAT=3,  I0PT=1,  I0NED=1,  IPRINT=0000 
INCON  H=0 ,  0BS1=0. ,YZONE=0. 

METHOD  RECT 

CONTROL  FINTIM=21 . ,  DELT=.l 

PRINT  XPOS,YPOS,ZPOS, XPOSM, YPOSM, ZPOSM 

XRINT  DS,DSM,DBPM,DBP,PITCHM, PITCH, XPOSM, YPOSM, XPOS.YPOS, ZPOSM, ZPOS,NDT 
XRINT  YPOSM, YPOS 

XRINT  THETAD, W, DEPTH, PITCH, XPOS, DEPTH, NDX,NDZ,NDT 

XAVE  THETA, W,Z, DEPTH, PITCH, DS, DB, BOWANG, STNANG 

XRAPHC DE=TEK618 )  TIME, D5 

XRAPHC  DE=TEK618 )  TIME, DEPTH 

XRAPHC  DE=TEK6 18 )  TIME.WDOT 

XRAPHC  DE=TEK6 18 )  TIME.W 

XRAPHC  DE=TEK618 )  TIME, THETDD 

XRAPHC DE=TEK6 18 )  TIME, THETAD 

XRAPHC  DE=TEK6 18 )  TIME, THETA 

XRAPHC  DE=TEK618 )  TIME, PITCH 

XRAPHC  DE  =  TEK6 18 )  TIME, BOWANG 

XRAPHC  DE  =  TEK6 18 )  TIME, STNANG 

xxxxxxxxxxxxxxxxxxxd  S  L  MODEL  FOR  LINEAR  SIMULATION  xxxxxxxxxxxxxxxxxxxx 
xxxxxxxxxxxxxxxxxxAND  NON-LINEAR  SIMULATION  VARYING  CONTROL  L AWxxxxxxxxxx 
x  LINEAR  MODEL/NON-LINEAR  MODEL 

D  COMMON/ BL0CK1/  MMINV C 6 , 6 ) ,  MMC6,6),  AAC 12 , 12 ) ,  BBC6,6) 

D  C0MM0N/BL0CK2/  BC 6 , 6 ) , AC  12 , 12 ) ,  UM0DC6 ) , GKKC6 , 21 ) 

D  COMMON/BLOCK3/  FC12),  FPC6 ) ,  UCFC4 ) 

D  C0MM0N/BL0CK4/  G4C 4 ) , GK4 C 4 ) , BRC 4  ) , HHC 4  ) 

D  C0MM0N/BL0CK5/  XDOTC 12 ) , XDOTXC 1 2 ) ,  XD0TUC6 ) 

FIXED  N, IA, IDGT , IER, LAST ,J,K,M,JJ,KK,I 
INTEGER 

ARRAY  WICAREAC  54) ,  XC12) 

x 

x 

X 
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CONST 

x 


x  LONGITUDINAL  HYDRODYNAMIC  COEFFICIENTS 
x 


CONST 

XPP  = 

i  XQQ  = 

,  XRR  = 

,  XPR  = 

XUDOT  = 

i  XWQ  = 

,XVP  = 

,XVR  = 

XQDS  = 

,XQDB= 

, XRDR= 

,xw  = 

XWW  = 

, XVDR  = 

, XWDS= 

, XWDB= 

XDSDS= 

, XDBDB= 

,XDRDR= 

,XQDSN= 

¥ 

XWDSN= 

,XDSDSN= 

X 

£ 

LATERAL 

HYDRODYNAMIC  COEFFICIENTS 

CONST 

YPDOT  = 

,YRDOT= 

» YPQ  = 

» YQR  = 

YVDOT  = 

>  YP  = 

.  YR  = 

» YLQ  = 

YWP  = 

,  YWR  = 

» YV  = 

,YVW  = 

YDR  = 

,  CDY  = 

X 

X 

£ 

NORMAL 

HYDRODYNAMIC  COEFFICIENTS 

CONST 

ZQDOT  = 

,ZPP  = 

» ZPR  = 

,ZRR  = 

ZWDOT  = 

,ZQ  = 

,ZVP  = 

,  ZVR  = 

ZW  = 

,ZW  = 

,  ZDS  = 

,  ZDB  = 

¥ 

ZQN  = 

» ZWN  = 

, ZDSN= 

,CDZ  = 

X 

ROLL  HYDRODYNAMIC  COEFFITIENTS 

CONST 

KPDOT  = 

,  KRDOT  = 

>  KPQ  = 

t  KOR  = 

KVDOT  = 

,  KP  = 

,  KR  = 

/  KVO  = 

KWP  = 

,  KWR  = 

,  KV  = 

,  KVW  = 

¥ 

KPN  = 

,  KDB  = 

X 

£ 

PITCH  HYDRODYNAMIC  COEFFICIENTS 

CONST 

MQDOT  = 

,  MPP  = 

,MPR  = 

r  MRR  = 

MWDOT  = 

.  MQ  = 

>  MVP  = 

,MVR  = 

MW  = 

,  MW  = 

» MDS  = 

,  MDB  = 

¥ 

MQN  = 

,  MWN  = 

, MDSN  = 

X 

YAW  HYDRODYNAMIC  COEFFICIENTS 

CONST 

NPDOT  = 

,  NRDOT= 

r  NPO  = 

>  NQR  = 

NVDOT  = 

,  NP  = 

,  NR  = 

r  NVQ  = 

NWP  = 

,  NWR  = 

,NV  = 

,  NVW  = 

¥ 

NDR  = 

X 

MASS  CHARACTERISTICS  OF  THE  FLOODED  MARK 

IX  VEHICLE 

CONST 

WEIGHT 

=  ,  BOY  = 

i  VOL  = 

,  XG  = 

YG  = 

,  ZG  = 

t  XB  = 

>  ZB  = 

IX  = 

,  IY  = 

,IZ  = 

,IXZ  = 

IYZ  = 

,  IXY  = 

,YB  = 

L  = 

,  RHO  = 

>  G  = 

,NU  = 

AO  = 

, KPROP  = 

, NPROP  = 

X1TEST 

X 

X 

DEGRUD= 

■0.0  , DEGSTN=  0.0 

CONST  XOBS1 =36 . 0 
CONST  ZOBS1 =  -12.0 


*  INPUT  INITIAL  CONDITIONS  HERE  IF  REQUIRED 
x 

INITIAL 

x 

*  DSAVE1=S9RTCCXP0SM-X0BS1)XCXP0SM-X0BS1)+. . . 

x  (ZP05M-Z0BS1 )xCZPOSM-ZOBSl ) ) 

x  DSAVEV=DSAVE1 

x  INITIALIZE  ALL  MATRICES  AND  ARRAYS  TO  ZERO 

NOSORT 

ORDDEP=20 . 0 
YORD=40 . 0 
D=0 
H=H+1 

IF  CH.EQ.l)  THEN 
N  =  6 

DO  2  J  =  1,N 
JJ  =  J+N 
DO  1  K  =  IfN 
KK=  K+N 
KKK  =  KK  +  N 
MMINVCJ,K)  =  0.0 
XCJ)  =  0.0 
XCJJ)  =  0.0 
XDOTCJ)  =  0.0 
XDOTCJJ)  =  0.0 
XDOTXCJ)  =  0.0 
XDOTX(JJ)  =  0.0 
XDOTUCJ)  =  0.0 

x  UMODCJ)  =  0.0 

MMC  J , K )  =  0.0 
BB< J, K)  =  0.0 
BC J,K)  =  0.0 
AACJ,K)=  0.0 
AAC  J J , KK)  =  0.0 
AAC  J , KK)  =  0.0 
AACJJ,K)=  0.0 
AC  J , KK)  =  0.0 
AC  J J , K)  =  0.0 
AC  J , K)  =  0.0 
AC  JJ , KK )  =  0.0 
GKKC J ,  K)  =  0.0 
GKKC J ,KK)  =  0 . 0 
GKKC  J . KKK)  =  0 . 0 

1  CONTINUE 

2  CONTINUE 
x 

x  INPUT  THE  LINEARIZATION  POINT  PARAMETERS 

UO  =6.0 
VO  =  0.0 
HO  =  0.0 
PO  =  0.0 
QO  =  0.0 
RO  =  0.0 
PHIO  =  0.0 
THETAO  =  0.0 
PSIO  =  0.0 
SUM  =  0.0 
JFLAG  =  0 
I  FLAG  =  0 
KFLAG  =  0 
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▼ 


*  INPUT  THE  MODEL  STATES  INITIAL  CONDITIONS 
x 

UM  =  6.0 
VM  =  0.0 
WM  =  0.0 
PM  =  0.0 
QM  =  0.0 
RM  =  0.0 
XPOSM  =0.0 
YPOSM  =0.0 
ZPOSM  =0.0 
PHIM  =  0.0 
THETAM  =0.0 
PSIM  =  0.0 
U  =6.0 
V  =0.0 
W  =0.0 
P  =0.0 
Q  =0.0 
R  =0.0 
XPOS  =0.0 
YPOS  =0.0 
ZPOS  =0.0 
PHI  =0.0 
THETA  =  0.0 
PSI  =0.0 
x 
x 

x  INPUT  THE  VEHICLE  INITIAL  CONDITIONS 

x 

x 

x  INITIALIZE  THE  CONTROLS 

* 

DELBOY=  0.0 
DB0Y=0 . 

DS=  0.0 
x  DSM=0 . 0 

*  DBM=0 . 0 

DB=0 . 0 
DR=  0.0 
DP.M=0 .0 
DRPM=0 . 0 
RPM  =  500.0 
LATYAH  =0.0 
HORPIT  =0.0 
x 

X 

MASS  =  WEIGHT/G 
x 

DIVAMP  =  DEGSTNXO. 0174532925 
RUDAMP  =  DEGRUDXO. 0174532925 
x 

x  DEFINE  LENGTH  FRACTIONS  FOR  GAUSS  QUADUTURE  TERMS 
x 

G4< 1 )  =  0.069431844 
G4( 2)  =  0.330009478 
G4( 3)  =  0.669990521 
G4  ( 4 )  =  0.930568155 
x 

x  DEFINE  WEIGHT  FRACTIONS  FOR  GAUSS  QUADUTURE  TERMS 
x 

GK4(  1  )  =  0.1739274225687 
GK4 ( 2 )  =  0.3260725774312 
GK4C3)  =  0.3260725774312 
GK4(4)  =  0.1739274225687 
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*  *  *  *****  *  ** 


DEFINE  THE  BREADTH  BB  AND  HEIGHT  HH  TERMS  FOR  THE  INTEGRATION 

BR(l)  =  75.7/12 
BRC  2 )  =  75.7/12 
BRC3)  =  75.7/12 
BRC  4 )  =  55.08/12 

HH( 1 )  =  16.38/12 
HH(2)  =  31.85/12 
HHC  3)  =  31.85/12 
HHC  4 )  =  23.76/12 

THE  LINEAR  PROPULSION  MODEL 


ETA  =  0 . 012#RPM/U0 
ETA  =  1.0 
RE  =  UOxL/NU 

CDO  =  .00385  +  Cl .296E-17)X(RE  -  1.2E7)XX2 

CT  =  Q.008XLXX2XETAXABSCETA)/CA0) 

CT1  =  0.008*L**2/(A0) 

EPS  =  -1 ,0+(SQRT(CT+l . 0 )  —  1 .  0 )  /  C  SORT (CT1  +  1.0)-1.0) 
XPROP  =  CDOxCETAxABSCETA)  -  1.0) 


CALCULATE  THE  MASS  MATRIX 

MMC1.1)  =  MASS  -<(RHO/2)*CL**3)XXUDOT) 

MMC 1 » 5 )  =  MASSXZG 
MMC 1,6)  =  -MASSXYG 

MMC2»  2)  =  MASS  -C C RHO/2 >X( LX*3 )XYVDOT ) 

MMC  2,4)  =  -MASSXZG  -C C RHO/2 >x( LXX4 )XYPDOT ) 
MMC 2, 6 )  =  MASSXXG  -  C CRH0/2)x( LXX4)XYRD0T) 

MM (3,3)  =  MASS  -  C (RHO/2)x( Lxx3)XZWD0T) 

MMC 3, 4)  =  MASSXYG 

MMC 3. 5)  =  -MASSXXG  -( (RH0/2)X( L*X4 )XZODOT) 

MM C  4 , 2 )  =  -MASSXZG  -  C CRH0/2)X(LXX4)XKVD0T) 
MMC 4, 3)  =  MASSXYG 

MM C  4 /  4 )  =  IX  -  C  C  RHO/2  )XCL*X5)XICPD0T) 

MMC  4,5)  =  -IXY 

MMC  4,6)  =  -IXZ  -C  C RHO/2 )*CLXX5)XKRD0T) 

MMC  5, 1 )  =  MASSXZG 

MMC 5 , 3 )  =  -MASSXXG  -C (RHO/2 )XC Lxx4 )xMWDOT) 
MMC  5.4)  =  -IXY 

MMC  5,5)  =  IY  -C(RH0/2)XCLXX5)XMQD0T) 

MMC  5»  6 )  =  -IYZ 

MMC6 , 1 )  =  -MASSXYG 

MMC6,2)  =  MASSXXG  -C C RHO/2 ) XC Lxx4 )xNVDOT) 
MMC  6 , 4 )  =  -IXZ  -  CCRH0/2)XCLXX5)XNPD0T) 

MMC 6  >  5 )  =  -IYZ 

MMC6 > 6 )  =  IZ  -  CCRH0/2)XCLXX5)XNRD0T) 


LAST  =  NXN+3XN 
DO  20  M  =  1 ,  LAST 
WKAREACM)  =0.0 
20  CONTINUE 
x 

IER  =  0 
IA  =  6 
IDGT  =  4 
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* 

# 

X 

X 

X 


t 


X 


X 


CALL  LINV2F(MM,N,IA,MMINV,IDGT,WKAREA,IER) 
CALCULATE  THE  A  MATRIX  FOR  THE  LINEAR  MODEL 


A( 1 , 1 )  = 


AC  1 , 25  = 
AC  1 , 35  = 
Ad, 4)  = 
Ad, 5)  = 

A(l, 6)  = 
A(l, 11)= 


RHO/2XLXX3X(XQDSXDSXQO+XQDB/2XDBPXQO+XRDRXROXDR)+. .  . 
RHO/2XLXX2X(XVDRXVOXDR+XWDSXDSXWO+XWDB/2XDBPXWO  +  ... 
2*U0x( XDSDSXCSXX2  +  XDBDB/2XDBPXX2  +  XDRDRXDRXX2) )+  ... 
RHO/2XLXX3XXQDSNXQOXDSXEPS+RHO/2XLXX2XCXWDSNXWOXDS+. . . 
2XXDSDSNXUOXDSXX2)XEPS+RHOXLXX2XUOXXPROP+RHO/2XLXX3X. . . 
XQDB/2XDBSXQO+RHO/2XLXX2XXWDB/'2XDBSXWO+RHOXLXX2XUOX. . . 
XDBDB/2XDBSXX2 

MASSXRO+RHO/2XLXX3XCXVPXPO+  XVRxRO)  +  RH0/2XLXX2X  ... 
(2XXVVXV0  +  XVDRXUOXDR) 

-MASSXQO  +  RHO/2XLXX3X(XWQXQO)+RHO/2XLXX2X(2XXWWXWO+. . . 
XWDSXDSXU0+(XWDB/2XDBP+XWDB/2XDBS)XU0  +XWDSNXUOXDSXEPS ) 
-MASSXYGXQO-MASSXZGXRO+  RH0/2*Lxx4x( 2XXPPXP0+XPRXR0 ) . . . 
+  RH0/2XLXX3X(XVPXV0) 

-MASSXWO+2XMASSXXGXQO  -MASSXYGXPO+RHO/2XLXX4X2XXQQXQO . . 
+RHO/2XLXX3X(XWQXWO+XQDSXDSXUO+XQDB/2XDBPXUO)+RHO/2X  . . 
LXX3XXQDSNXUOXDSXEPS+RHO/2XLXX3XXQDB/2XDBSXUO 
MASSXVO+2XMASSXXGXRO-MASSXZGXPO+RHO/2XLXX4X(2XXRRXRO . . . 
+  XPRXPO )  +  RH0/2XL«X3X( XVRxVO  +  XRDRxUOXDR) 

-(WEIGHT  -  BOY)XCOS(THETAO) 


A( 2, 1 )  =  -MAS5XRO+RHO/2XLXX3X(YPXPO+YRXRO)+RHO/2XLXX2X(YVXVO+. . . 
2XYDRXUOXDR) 

A( 2 , 2 )  =  RHO/2XLXX3XYVQXQO+RHO/2XLXX2X(YVXUO+YVWXWO) 

A(2, 3)  =  MASSXPO+  RHO/2XLXX3X(YWPXPO+YWRXRO)+RHO/2XLXX2XYVWXVO 
AC  2, 4 )  =  MASSXHO-MASSXXGXQO+2XMASSXYGXPO+RHO/2XLXX4XYPQXQO+. . . 
RH0/2XLXX3XCYPXU0+  YWPXWO) 

A( 2, 5 )  =  -MASSXXGXPO-MASSXZGXRO+RHO/2XLXX4X(YPQXPO+YQRXRO)  +... 
RH0/2XLXX3XYVQXV0 

A(2,6)  =  -MASSXUO+2XMASSXYGXRO-MASSXZGXQO+RHO/2XLXX4XYQRXQO  +... 

RHO/2XLXX3X(YRXUO  +  YWRXWO) 

A( 2, 10  )=  (WEIGHT  -  BOY)XCOSCTHETAO)XCOS(PHIO) 

A(2, 11 )=  -(WEIGHT  -  BOY)XSIN(THETAO)XSIH(PHIO) 

A( 3, 15  =  MASSXQO+RHO/2XLXX3XZQXQO+RHO/2XLXX2X(ZWXWO+2XUOXZDSXDS. 

+2XU0XZDB/2XDBP+(ZWNXW0+2XZDSNXU0XDS)XEPS)+RH0/2XLXX3X. 
ZQNXQOXEPS+  RHO/2XLXX2X2XUOXZDB/2XDBS 
A (3, 2)  =  -MASSXPO+RHO/2XLXX3X(ZVPXPO+ZVRXRO)+RHO/2XLXX2X2XZVVXVO 
A( 3, 3 )  =  RH0/2XLXX2X(ZWXU0  +  ZWNXUOXEPS) 

A( 3 , 4 )  =  -MASSXV0-MASSXXGXR0+2XMASSXZGXP0+  RH0/2XLXX4X(2XZPPX. . . 

PO  +  ZPRxRO)  +  RHO/2XLXX3XZVP*VO 
A( 3, 5)  =  MASSXUO  -  MASSXYGXRO+2XMASS*ZGXQO+RHO/2XLXX3XZQXUO  +... 
RHO/2XLXX3XZQNXUOXEPS 

A(3, 6 )  =-MASSxXGxP0-MASSxYGXQ0+RHO/2xL*x4X(ZPRxP0+2xZRRxR0)+. . . 


*■ 
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FILE:  TX 


DSL 


A1 


A(3,10)= 
A(3, 11)= 

A(4, 1 )  = 

A(4,2)  = 
A(4,3)  = 
A(4,4)  = 
A(4,5)  = 
A(4,6)  = 
A(4# 10)= 
A( 4 . 11 )  = 

A( 5  > 1 )  = 

A(5,2)  = 

A( 5»  3 )  = 
A(5, 4 )  = 

A( 5, 5 )  * 

A(5,6)  = 

A(5,10)= 
A( 5» 1 1 )  = 

A(6,l)  = 

A( 6 , 2 )  = 

A(6, 3)  = 

A(6,4)  = 

A(6 , 5)  = 

A(6,6)  = 

A<6, 10)= 
A(6,ll)= 


RHO/2XLXX3XZVRXVO 

-(WEIGHT  -  BOY)XCOS(THETAO)XSIN(PHIO) 

-(WEIGHT  -  BOY )XSIN( THETAO )xCOS( PHIO ) 

MASSXYGXQO  +  MASSxZGXRO  +  RHO/2XLXX4X(KPXPO  +  ... 

KRXRO )+RH0/2XL*x3x(KVXV0+2XU0X(KDB/2xDBP-XDB/2xDBS) )+ . 
RHO/2XLXX3XUOXKPROP+  RH0/2XLXX4XKPNXP0XEPS 
-MASSXYGXPO  +  RH0/2XLXX4XKVQXQ0  +  RH0/2XLXX2X( KVXUO  -  . . 
+  KVWXWO) 

-MASSXZGXPO  +  RHO/2XLXX4X(KWPXPO  +  KWRxRO)  +  ... 
RHO/2XLXX3XKVHXVO 

-IXYXRO  +  IXZXQO  -  MASSXYGXVO  -  MASSXZGXWO  +  . . . 
RHO/2XLXX5XKPQXQO  +  RH0/2XLXX4X( KPXUO+KWPXWO ) 

-IZXRO  +  IYXRO  +  2XIYZXQ0  +  IXZXPO  +  MASSXYG»UO  +... 
RH0/2XLXX5X( KPQxPO  +  KQRXRO)  +  RHO/2XLXX4XKVQXVO 
-IZXQO  +  IYXQO  -  2XIYZXR0  +  MASSXZGXUO  +  .  .  . 
RH0/2XLXX5XKQR*Q0  +  RH0/2*L  XX4«( KRXUO+KWRxWO ) 
-(YGXHEIGHT-YBXBOY)XCOS(THETAO)XSIN(PHIO) . . . 
-(ZGXWEIGHT-ZBXBOY)XCOS(THETAO)XCOSCPHIO) 
-(YGXHEIGHT-YBXBOY)XSIN(THETAO)XCOS(PHIO) . .  . 
+(ZGXW£IGHT-ZBXB0Y)XSIN(THETA0)XSIN(PHI0) 

-MASSXXGXQO  +  RH0/2XLXX4XMQXQ0  +  RH0/2XLXX3XMWXW0  +... 
RH0/2XLX*3XU0X(MDSXDS+MDB/2XDBP)  +  RH0/2XL XX4XMQNXQ0X . 
EPS  +  RHO/2XLXX3X(MWNXWO  +  2xMDSNxU0XDS)xEPS+ . . . 
RH0/2XLXX3XU0XMDB/2XDBS 

MASSXXGXPO  +  MASS»ZG»RO  +  RHO/2XLXX4X(MVPXPO  +  ... 
MVRxRO )  +  RHOXLXX3XMVVXVO 

-MASSXZGXQO  +  RHO/2XLXX3XMWXUO  +  RHO/ZXLXX5XMWNXUOXEPS 
-IXXRO  +  IZXRO  -  IYZXQO  -  2XIXZ»P0  +  MASSXXGXVO  +  . . . 
RHO/2XLXX5X(2XMPPXPO  +  MPRXRO)  +  RHO/2XLXX4XMVPXVO 
IXYXRO  -IYZXPO  -  MASSXXGXUO  -MASSXZGXWO  +  RH0/2X... 
LXX4XMQXU0  +  RH0/2XL XX4XMQNXU0XEPS 

-IXXPO  +  IZXPO  +  IXYXQO  +  2XIXZXR0  +  MASSXZGXVO  +... 
RHO/2XLXX5X(MPRXPO+2XMRRXRO)+RHO/2XLXX4XMVRXVO 
( XG*WEIGHT-XBXBOY)XCOS( THETA 0 )XSIN(PHIO) 
(XGxWEIGHT-XB«BOY)*SIN(THETAO )XCOS( PHIO  )  -  ... 
(ZG*WEIGHT-ZB*BOY )XC0S( THETAO) 

-HASSXXGXRO  +  RH0/2XLXX4X(NPXP0  +NRXRO)  +  RHO/2*... 
LXX3X(NVXVO+2XNDRXUOXDR)+RHOXLXX3XUOXNPROP 
-MASSXYGXRO  +  RHO/2XLXX4XNVQXQ0  +  RH0/2XLXX3X( NVXUO+ . . 
NVWXWO) 

MASSXXGXPO  +  MASSXYGXQO  +  RH0/2XLXX4X( NWPXPO+NWRXRO )+ . 
RH0/2XLXX3XNVWXV0 

-IYXQO  +  IXXQO  +  2XIXYXP0  +IYZXRO  +  MASSXXGXWO+ . . . 
RH0/2XLXX5XNPQXQ0  +  RHO/2XLXX4X(NPXUO+NHPXWO) 

-IYXPO  +  IXXPO  -  2XIXYXQ0  -  IXZXRO  +  MASSXYGXWO+ . . . 
RH0/2XLXX5X(NPQXP0+NQRXR0)  +  RH0/2XLXX4#NVQXV0 
IYZXPO  -IXZXQO  -  MASSXXGXUO  -MASSXYGXVO  +  . . . 
RHO/2XLXX5XNQRXQO  +  RH0/2XLXX4X( NRXUO  +NWRXWO ) 

( XGXWEIGHT-XBXB0Y)XC0S( THETAO )XCOS( PHIO) 
-(XGXHEIGHT-XBXB0Y)XSIN(THETA0)XSIN(PHI0)  +.  . . 
(YGXWEIGHT-YBXBOY)XCOS(THETAO) 


82 


*  *  * 


A(7 , 1 )  =  COS (PS 10 )XC0S(THETA0) 

A( 7 , 2 )  =  COS(PSIO)XSIN(THETAO)XSIN(PHIO)  -  SIN< PSI 0 JXCOSCPHIO ) 
A(7 , 3 )  =  COS(PSIO)XSIN(THETAO)XCOS(PHIO)  +  SI N( PSI 0  )XSIN( PHI  0 ) 
A(7,10)=  VOXCOS( PSI 0)xSIN(THETA0)XCOS(PHI0)  +  VOKSIN(PSIO)*. . . 

SIN(PHIO)  -  WO*COS(PSIO)*SIN(THETAO)*SIN(PHIO)  +  ... 
WOXSIN(  PSI 0)XC0S( PHIO) 

A( 7 , 11 )  =  -UOxCOS( PSIO)xSIN(THETAO)  +  VQXC0S(PSI0)xC0S(THETA0)x 
SIN(PHIO)  +  WO»COS( PSI 0 )*COS( THETAO )xCOS( PHI  0 ) 
A(7,12)=  -UOXSIN(PSIO)XCOS(THETAO)  -  VOXSINCPSI 0 )xSIN( THETAO )X 
SIN(PHIO)  -  VO*COS( PSI 0)*COS( PHIO)  -  WOXSIN(PSIO)X. . . 
SIN(THETAO)XSINCPHIO)  +  WOxcOS( PSI 0 )xSIN( PHI  0 ) 

* 

A( 8 , 1 )  =  SI N(PSIO)xCOS( THETAO) 

A( 8 , 2 )  =  SIN(PSIO)xSIN(THETAO)XSIN(PHIO)  +  COS(PSIO)XCOS(PHIO) 
A ( 8 , 3)  =  SIN( PSI 0)XSIN( THETAO )XCOS( PHIO )  -  COSCPSIO)XSIN(PHIO) 
A(8,10)=  VOXSIN(PSIO)XSINCTHETAO)XCOSCPHIO)  -  VOXCOS(PSIO)X. . . 

SIN(PHIO)  -  W0XSIN(PSI0)XSIN(THETA0)XSIN(PHI0)  -  ... 

WOXCOSC PSI 0)XCOS( PHIO) 

A(8 , 1 1 ) =  -UOxSIN(PSIO)XSIN (THETAO)  +  VOXSIN(PSIO)XCOS(THETAO)X 
SIN(PHIO)  +  WOXSIN(PSIO )XC0S( THETAO ) XCOS( PHI 0  ) 

A( S , 12 ) =  UOXCOS(PSIO)XCOS(THETAO)  +  VOXCOS( PSI 0 ) XSIN( THETAO )X . 

SIN (PHIO)  -  VOXSIN(PSIO)XCOS(PHIO)  +  MOXCOS( PSI 0 )X . . . 
SIH(THETAO)XCOS(PHIO>  +  W0XSIN(PSI0)XSIN(PHI0) 
x 

A ( 9 , 1 )  =  -SIN(THETAO) 

A( 9 , 2 )  =  COS(THETAO)XSIN(PHIO) 

A(9, 3)  =  COS( THETAO )XCOS( PHIO) 

A(  9 , 1 0  )  =  V OXCOS ( THETAO )XCOS( PHI 0)-W0XCOS( THETAO )XSIN( PHIO) 

A( 9 , 1 1 ) =  -UO*COS( THETAO )-V0XSIN(THET AO )XSIN( PHIO)  -... 

WOXSIN( THETAO )XCOS(PHIO) 
x 

A ( 1 0 , 4 )  =  1.0 

A (1 0 , 5  )  =  SIN(PHIO) XT AN (THETAO) 

A(10, 6)  =  COS ( PHIO ) XT AN( THETAO) 

A(10,10)=  00*C0S( PHIO)xTAN(THETAO)  -  ROXSIN(PHIO)XTAN(THETAO) 
A(10,ll)=  OOXSIN(PHIO)/COS(THETAO)Xl.O/COS(THETAO)  +  ... 
ROXCOSC PHI 0)/COS( THETAO) XI . 0/COS( THETAO) 
x 

A( 1 1 , 5 )  -  COS(PHIO) 

A(ll, 6)  =  -SIH(PHIO) 

Adi, 10)=  -QOXSIN(PHIO)  -  ROXCOS(PHIO) 
x 

A( 12 , 5 )  =  SIN(PHIO)/COS(THETAO) 

A ( 1 2 , 6 )  =  COS(PHIO)/COS(THETAO) 

A (12, 10)=  QOXCOS( PHI  0 )/COS( THETAO )-ROXSIN( PHI 0)/COS( THETAO) 

A( 12, 11 ) =  QOXSIN(PHIO)/COS(THETAO)XTAH(THETAO)  +  ... 

ROXCOS( PHI 0)/C0S( THETAO )XTAN( THETAO) 

WRITE(10,200)((A(I,J),J=1, 12), 1=1,12) 
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)K  )$(  )K  )K  ) tf  )K  )K  )K)K  )♦< 


CALCULATE  THE  B  MATRIX 


BC 1 , 1 )  =  RHO/2XLXX3XXRDRXUOXRO+RHO/2XLXX2XCXRDRXUOXVO+UOXX2X. . 
2XXDRDRXDR ) 

B ( 1 , 2 )  =  U0XQ0XXQDB/2  +  UOXWOxXWDB/2  +  U0**2*XDBDB*DBS 
BC 1 , 3)  =  UOXQOxXQDB/2  +  UOXWOXXWDB/2  +  U0XX2XXDBDBXDBP 
B(l,4)  =  UOXQOXXQDS  +  UOXWOXXWDS  +U0XX2X2XXDSDSXDS+RH0/2XLXX3X 
XQDSNXUOXQOXEPS  +  RHO/2XLXX2XCXWDSNXUOXWO  +  2XXDSDSN* 
U0XX2XDS)XEPS 

BC 1 , 5 )  =  RHO/2XLXX2X0.12XCD0X0.12XRPM 
B(  1 >6 )  =  SINCTHETAO) 

BC  2, 1 )  =  RH0/2XLXX2XYDRXU0XX2 
BC  2 , 6 )  =  -COSCTHETAO)XS1NCPHIO) 

B( 3, 2)  =  UOXX2XZDB/2XRHO/2XLXX2 
BC  3, 3 )  =  UOXX2XZDB/2XRHO/2XLXX2 

B( 3>  4 )  =  UOXX2XZDSXRHO/2XLXX2  +  RH0/2XLXX2XZDSNXU0XX2XEPS 
BC3, 6 )  =  -COS C  THETA 0 )XCOS C  PHI  0 ) 

B(4,2)  =-RH0/2xlxx3xU0x*2xKDB/2 
B( 4, 3)  =  RHO/2XLXXSXUOXX2XKDB/2 

BC  4 , 6  )  =  -YBXCOS(THETAQ)XCOSCPHIO)  +  ZBxCOSCTHETAO)XSINCPHIO) 

BC  5  >  2 )  =  RH0/2xlxx3xU0xx2xMDB/2 
BC  5, 3)  =  RH0/2XLXX3XU0XX2XMDB/2 
B  C  5 , 4  )  =  RH0/2XLXX3X(U0XX2XMDS+MDSNXU0XX2XEPS) 

B ( 5, 6 )  =  XBXCOS(THETAO)XCOSCPHIO)  +  ZBxSINC THETAO ) 

B  C6 , 1 J  =  RHO/2XLXX3XNDRXUOXX2 

BC6 ,6 )  =  -XBXCOSCTHETAO)XSIN(PHIO)  -  YBxSINCTHETAO) 

FORMULATE  THE  A  AND  B  MATRIX  FOR  STATE  SPACE  REPRESENTATION 

MULTIPLY  MMINV  AND  DF/DX 

DO  80  I  =  1,6 
DO  70  J  =  1,6 
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FILE:  TX 


DSL 


A1 


SUM  =  0.0 

DO  60  K  = 

1,6 

SUM  =  SUM 

+  MMINV(I,K)*ACK, J) 

60 

CONTINUE 

AA ( I , J )  = 

SUM 

70 

CONTINUE 

80 

X 

CONTINUE 

X 

*  MULTIPLY  MMINV  AND  DF/DZ 

X 

X 

DO  50  I  =  1,6 

DO  40  J  =  7,12 
SUM  =  0.0 

DO  30  K  =  1,6 

SUM  =  SUM  +  MMINV(I,K)*A(K, J) 

30  CONTINUE 

AA(I,J)  =  SUM 
40  CONTINUE 

50  CONTINUE 

X 
X 

DO  5  I  =  7,12 
DO  6  J  =  1,12 
A  A  ( I ,  J  )  =  A  (  I ,  J  ) 

6  CONTINUE 

5  CONTINUE 

X 
x 

WRITE(10,200)((AA(I,J),J=1,12),I=1,12) 
200  FORMAT (  6E12.4) 

X 

x 

X  MULTIPLY  MMINV  AND  DF/DU 

X 

x 

DO  110  I  =  1,6 
DO  100  J  =  1,6 


SUM  =0.0 

DO  90  K  =  1,6 

SUM  =  SUM  +  MMINV( I,K)#BCK,J) 

90 

CONTINUE 

100 

BB(I,J)  =  SUM 
CONTINUE 

110 

X 

CONTINUE 

X 

HRITE(  9 , 300 ) ( (BB( I , J ) 

,J=1,6),I=1,6) 

300 

X 

FORMAT(6  E12 . 4 1 

X 

DO  405  I  =  1,6 

READ  ( 2 , 401 )  ( GKK( I , J  ) , 

J  =  1 , 21 ) 

405 

WRITEC5.401K GKK( I , J ) , 

J  =  1 ,21  ) 

401 

X 

FORMATC  3E20 .10) 

es 


ELSE 
END  IF 
X 

CALL  DADS < INFO, ISTRAT ,I0PT ,IONED,IPRINT, I GRAD, NDV , NCON, DX, . . . 

VLB, VUB, OBJ, GW, IDG, NGT , IC, DF, AW, NRA, NCOLA, WK,  NRWK,  .  .  . 
IWK, NRIWK) 

IF  (INFO.EQ.  0)  DELPRT=0 . 2 
x 

DERIVATIVE 

NOSORT 

X 

X 

LATYAW  =0.0 
NORPIT  =0.0 

x 

xxxxxL INEAR  MODEL****************************************************** 


x  CALCULATE  BB*U  PART  OF  XDOT  =  AA*X  +  BB*U 
x 

DO  10  J  =  1,6 
SUM  =  0.0 
DO  15  K  =  1,6 

SUM  =  SUM  +  BBC  J , K)*UMOD(K) 

15  CONTINUE 

XDOTUCJ)  =  SUM 
10  CONTINUE 
X  CALCULATE  AAXX 

DO  21  J=  1,12 
SUM  =0.0 
DO  25  K  =  1,12 

SUM  =  SUM  +  AACJ»K)*XCK) 

25  CONTINUE 

XDOTX(J)  =  SUM 
21  CONTINUE 

X  CALCULATE  XDOT  =  AA*X  +  BB*U 
DO  31  J  =  1,6 

XDOT(J)  =  XDOTX(J)  +  XDOTUCJ) 

31  CONTINUE 

DO  35  J  =  7,12 

XDOT  C  J )  =  XDOTXCJ) 

35  CONTINUE 

x 

UDOTM  =  XDOT  Cl) 

VDOTM  =  XDOT  C  2 ) 

WDOTM  =  XDOT  C  3 ) 

PDOTM  =  XDOT C  4) 

QDOTM  =  XDOT (5) 

RDOTM  =  XD0TC6) 

XDOTM  =  XDOT C  7 ) 

YDOTM  =  XDOT  C  8 ) 

ZDOTM  =  XDOT  C  9 ) 

PHMDOT  =  XDOT  CIO) 

THETMD=  XDOT  C 1 1 ) 

PSMDOT  =  XDOT (12) 
x  WRITEC8,600) 

*  INTEGRATE  XDOT  TO  GET  THE  STATE  VECTOR  X 
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X  *  XX 


UM  = I MTGRL C 6 . 0,  UDOTM) 

VM=  INTGRL (0.0,  VDOTM) 

WM=  INTGRL (0.0,  WDOTM) 

PM=  INTGRL (0.0,  PDOTM) 

QM=  INTGRL (0.0,  QDOTM) 

RM=  INTGRL (0.0,  RDOTM) 

XPOSM  =  INTGRL ( 0 . 0,  XDOTM) 

YPOSM  =  INTGRL ( 0.0,  YDOTM) 

ZPOSM  =  INTGRL (0.0,  ZDOTM) 

PHIM  =  INTGRL (0.0,  PHMDOT) 

THETAM  =  INTGRL (0.0,  THETMD) 

INTGRD  =  <UM*UM+VM*VM+WMXWM+PM*PM+<3M*QM+RM*RM+.  .  . 

XPOSM*XPOSM+(YPOSM-YORD)*( YPOSM- Y0RD1+ . . . 
(ZPOSM-ORDDEP )*( ZPOSM- ORDDEP )+  PHIM#PH1M+. . . 
THETAM*THETAM+PSIM*PSIM)  +  CDSM*DSM+DBSM*DBSM)+ . . 
( DBPM*DBPM)+( DRMxDRM) 

OBJ1  =  INTGRL(0.,(0.5)*INTGRD) 

OBJ  =  OBJ1 

PSIM  =  INTGRL (0.0,  PSMDOT) 


X(l)  =  UM 
X( 2 )  =  VM 
X( 3 )  =  WM 
X( 4  )  =  PM 
X(5)  =  QM 
X( 6 )  =  RM 
X( 7 )  =  XPOSM 

X( 8 )  =  YPOSM 
X( 9 )  =  ZPOSM 
X(10)  =  PHIM 
X(ll)  =  THETAM 
X( 12 )  =  PSIM 

ZDEPTH  =  ZORD  -  X(9) 

THMANG  =  X(ll)*57.3 
UMOD(l)=DRM 
UMOD( 2 )  =  DBSM 
UMOD( 3 )  =DBPM 
UMOD( 4 )  =  DSM 
UMOD( 5 )  =  DRPM 
UMOD( 6 )  =  DBOY 

PHANG=PHIM/0. 0174532925 
THMANG=THETAM/0 . 0174532925 
PSMANG=  PSIM/  0.0174532925 
PITCHM=THMANG 
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JK****************  *  *  * 


xxxxxxCONTROL  lawxxxxxxxxxxxxx 


xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx 


DBS  =  UM0DC2) 

DBP  =  UMODC3) 

DS  =  UM0DL4) 

Ess  =  -tffiLixu  ;  0«<|.2)«v  ^‘fiKJSsVoK^^EXYMS'.... 

!K^'i2S<I.»>5tilTAH>I“o««t.lY>«UH«»<2>  *  0KK(2. 1*>«.  ■  • 
nr  .  ^4i>»  13* lRS!7*5iBS.4*’S«iii?M«'*  - • ' 

,;*OK1«!,17)«UHOD(2)  *  GPCKCJ.W.  •  ■ 

ifSIS'i  *  ckku.xx.  . 

UM0DC3)  +  GKK(4,19)XUM0D(4)) 

PUT  IN  STERN  AND  BOW  PLANE  STOPS 

IF(ABSCDBS). GT.0.6)  THEN 
DBS  =  0.6*ABS(DBSVDBS 

IFCABSLDBP) .GT.0.6)  THEN 
DBP  =  0.6XABS(DBP)/DBP 

IFCABS(DS). GT.0.6)  THEN 
DS  =  0.6XABS(DS)/DS 
ENDIF 

XXXXXNON-LINEAR  MODELXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX 

PROPULSION  MODEL 


SIGNU  =1.0  ,  „ 

IF  (U.LT.O.O)  SIGNU  =  _l-0 
IF  CABS(U) . LT .XI TEST)  U  =  X1TEST 

IFG(RPM.LT°0.0)  SIGNN  =  -1.0 
ETA  =  0.0I2XRPM/U 
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*  *  *  *******Ul  X  X  XX  X  X  X 


FILE;  TX 


DSL 


A1 


RE  =  UXL/NU 

CDO  =  .00385  +  ( 1 . 296E-17 )X(  RE  -  1.2E7)XX2 

CT  =  0.008Xl*X2*ETA*ABS(ETA)/CAO) 

CT1  =  0 . 008*LX*2/(AO) 

EPS  =  -1 .  0+SIGNN/SIGNUX(  SQRKCT+l .  0  )-l .  0  )/(SQRT(CTl+l .  0)-l .  0 ) 
XPROP  =  CDOK(ETAXABSCETA)  -1.0) 


CALCULATE  THE  DRAG  FORCE,  INTEGRATE  THE  DRAG  OVER  THE  VEHICLE 
INTEGRATE  USING  A  4  TERM  GAUSS  QUADUTURE 


LATYAH  =0.0 
NORPIT  =  0.0 
DO  500  K  =  1,4 

UCF(K)  =  SQRT(  ( V+G4(K )XR*L )xx2  +  CW-G4(K)xQXL )XX2) 
IF(UCF(K) .GT.1E-10)  THEN 

TERMO  =  (RH0/2mCDY*HHCK)*CV+G4CIO*RXL)XX2  +.  .  . 

CDZxBRnOX(W-G4<K)XQ*L)**2) 

TERM1  =  TERM0X(V+G4(K)XRXL)/UCFCK) 

TERM2  =  TERM0XCW-G4(K)XQXL)/UCFCK) 

LATYAW  =  LATYAW  +  TERM1XGK4CIOXL 
NORPIT  =  NORPIT  +  TERM2XGK4CIOXL 
END  IF 

0  CONTINUE 


FORCE  EQUATIONS 


LONGITUDINAL  FORCE 

FP(1)  =  MASSXV*R  -  MASSXWXQ  +  MASSXXGXQXX2  +  MASSXXGXRXX2- . . . 

MASSXYGXPXQ  -  MASSXZGXPXR  +  (RH0/2)XLXX4X(XPPXPXX2  +  ... 
XQQXQXX2  +  XRRXRXX2  +  XPRXPXR)  +(RH0/2)XLXX3X(XWQXWXQ  +.. 
XVPXVXP+XVRXVXR+UXQX(XQDSXDS+XQDB/2XDBP)+XRDRXUXRXDR)+. . . 
(RH0/2)XLXX2XCXVVXVXX2  +  XWHxNxx2  +  XVDRxuxVxDR  +  Uxwx... 
(XWDSXDS+XWDB/'2XDBP)+U*X2XLXDSDSXDSXX2+XDBDB/2XDBPXX2+. .  . 
XDRDRxDRX*2) )-(WEIGHT  -B0Y)xSIN (THETA)  +(RH0/2)XLXX3X  ... 

XQDSHXUXQXDSXEPS+(RH0/2)XLXX2X(XNDSNXUXWXDS+XDSDSNXUXX2X. 
D$xx2 ) XEPS  +(RHO/2)XLXX2XUXX2XXPROP+RHO/2XLXX3XUXQX  ... 
XQDB/2XDBS  +RH0/2XLXX2XUXX2XXDBDB/2XDBSXX2+  ... 
RH0/2XLXX2XXWDB/2XDBSXUXW 

LATERAL  FORCE 

FPC2)  =  -MASSXUXR  +  MASSXXGXPXQ  +  MASSXYG*RXX2  -  MASSXZGXQXR  +.. 

( RH0/2 )*L*x4X( YPQXPXQ  +  YQRXQXR)+(RH0/2)XLXX3X(YPXU*P  +.. 
YRXUXR  +  YVQXVXQ  +  YWPXWXP  +  YWRXWXR)  +  ( RH0/2 )XLXX2X 
(YVXUXV  +  YVWXVXW  +YDRXUXX2XDR)  -LATYAH  +(WEIGHT-BOY)X . . . 
COS(THETA)*SIN(PHI) 
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NORMAL  FORCE 

FPL3)  =  MASSXUXQ  -  MASSXVXP  -  MASSXXGXPXR  -  MASS»YGXQxR  + 

MASSXZGXPXX2  +  MASSXZGXQXX2  +  LRH0/2)XLXX4X(ZPPXPXX2  +... 
ZPRxpxR  +  ZRRXRXX2)  +  L RH0/2)XLXX3XLZQXUXQ  +  ZVPxvxp  +... 
ZVRXVXR)  +(RH0/2)XLX»2X(ZWXUXW  +  ZVVXVXX2  +  UXX2XLZDS* . . . 
DS+ZDB/2XDBP) )-NORPIT+L WEIGHT- BOY) XCOSL  THETA ) XCOSL  PHI )+ . . . 
(RH0/2)XLXX3XZQNXUXQXEPS  +(RH0/2)XLXX2X(ZWNXUXW  +ZDSNX . . . 
UXX2XDS)XEPS+  RH0/2XLXX2XUXX2XZDB/2XDBS 

ROLL  FORCE 

FPL  A)  =  -IZXQXR  +IYXQXR  -IXYxPxr  +IYZXQXX2  -IYZXRXX2  +IXZxpxQ  +.. 

MASSXYGXUXQ  -MASSXYGXVXP  -MASSXZGXWXP4LRH0/2)XLXX5X(KPQX. . 
PXQ  +  KQRXQXR)  +(RH0/2)XLXXAX(KPXUXP  +KRXUXR  +  KVQXVXQ  +.. 
KWPXWXP  +  KWRXWXR)  +<RH0/2)XLXX3X(KVXUXV  +  KVWxVxW)  +  ... 

(YGXHEIGHT  -  YB*BOY)XCOS(THETA)XCOS(PKI )  -  ( ZGXWEIGHT  -  .. 
ZBXBOY)XCOS(THETA)XSINCPHI)  +  (RH0/2)XLXXAXKPNXUXPXEPS  +.. 
(RH0/2)xlxx3xuxx2XKPR0P  +MASSXZGXUXR+  ... 

RH0/2XLXX3XUXX2XCKDB/2XDBP-KDB/2XDBS) 

PITCH  FORCE 

FPL5)  =  -IXXpXR  +IZXPXR  +IXYXQXR  -IYZxpxQ  -IXZXPXX2  +IXZXRXX2  -.. 

MASSXXGXUXQ  +  MASSXXGXVXP  4  MASSxZGxvxR  -  MASSxZGxWxQ  4... 
(RH0/2)XLXX5X(MPPXPXX2  +MPRXPXR  4MRRxRxx2)41RHO/2)xlxx4x  .  . 
LMQXUXQ  4  MVPXVXP  4  MVRXVXR)  4  (RH0/2)XLXX3X(MWXUXH  4  ... 
MVVXVXX24UXX2X(MDSXDS4MDB/2XDBP))4  NORPIT  -LXGxWEIGHT-  ... 
XBXB0Y)XC0S(THETA)XC0S(PHI)  4... 

(RH0/2)XLXX3X(MWNXUXW4MDSHXUXX2XDS)XEPS4  RH0/2xlxx3x. . . 
UXX2XMDB/2XDBS-(ZGXHEIGHT-ZBXB0Y)XSINCTHETA) 

YAW  FORCE 

FPL 6)  *  -IYXPXQ  4IXXPXQ  41XYXPXX2  -IXYXQXX2  4IYZXPXR  -IXZXQXR  -.. 

MASSXXGXUXR  4  MASSXXGXWXP  -  MASSxYGxVxR  4  MASSXYGXWXQ  4... 
LRH0/2)XLXX5XLNPQXPXQ  4  NQRXQXR )  4LRH0/2)XLXX4XLNPXUXP4 . . . 
NRXUXR  4  NVQXVXQ  4NWPXWXP  4  NWRXWXR)  4LRH0/2)XLXX3X(NVX . . . 
UXV  4  NVWXVXW  4  NDRXUXX2XDR)  -  LATYAW  4  L XGXWEIGHT  -  ... 

XBXB0Y)XC0SLTHETA)XSINLPHI)4LYGXWEIGHT)XSINLTHETA) . . . 
4LRH0/2)XLXX3XUXX2XNPR0P-YBXB0YXSINLTHETA) 

IFLZ.EQ.50.0)THEN 

WRITE  L8,500)LFPLI),  I  =  1,6) 

Z  =  0.0 
END  IF 

NOW  COMPUTE  THE  FL 1-6 3  FUNCTIONS 

DO  600  J  =  1,6 

FLJ)  =  0.0 
DO  600  K  =  1,6 

FLJ)  =  MMINVIJ,K)XFPLK)  4  FLJ) 

CONTINUE 

THE  LAST  SIX  EQUATIONS  COME  FROM  THE  KINEMATIC  RELATIONS 

FIRST  SET  THE  DRIFT  CURRENT  VALUES 

UCO  =  0.0 
VCO  =  0.0 
WCO  =0.0 
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*  *  *  * 


*  INERTIAL  POSITION  RATES  F(  7-9) 
x 

F(7)  =  UCO  +  UXC0S(PSI)XC0S(THETA)  +  Vx(COS(PSI )XSIN(THETA)X . . . 
SIN (PHI)  -  SIN(PSI)xCOS(PHI))  +  MX(COS( PSI )*SIN( THETA)* . 
COS(PHI)  +  SIN(PSI)XSIN(PHD) 
x 

FC8)  =  VCO  +  UXSIN(PSI)XC0S( THETA)  +  Vx(SIN(PSI )XSIN( THETA )x . . . 

SIN(PHI)  +  COSCPSI )XC0S(PHI) )  +  WX(SIN(PSI)XSIN(THETA)X. 
COS(PHI)  -  COS ( PSI )XSIN(PHI)) 
x 

FC9)  =  WCO  -  UXSIN(THETA)  +VxCOS(THETA)XSIN(PHI)  +WxCOS(THETA)x 
COS(PHI) 
x 

x  EULER  ANGLE  RATES  FC10-12) 
x 

FC 10)  =  P  +  <3XSIN(PHI)XTAN(THETA)  +  RXCOS ( PHI )XTAN( THETA) 
x 

F( 11 )  =  QXCOS(PHI)  -  RXSIN(PHI) 
x 

F(  12)  =  QXSIN(  PHI  )/COS<  THETA)  +  RXCOS(PHI)/COS(THETA) 

x 

x 

x  IF  (Z.EQ.l .0)WRITE  (9, 500 ) (F( I ) ,  I  =  1,12) 

X00  FORMAT ( 6  E12 . A ) 
x  Z  =  Z  +  1 
x 
x 


UDOT  = 

F(  1 ) 

VDOT  = 

F(2) 

UDOT  = 

F(3) 

PDOT  = 

F(4) 

QDOT  = 

F(  5 ) 

RDOT  = 

F(6 ) 

XDOTA= 

F(  7  ) 

YDOT  = 

F(  8 ) 

ZDOT  * 

F(9) 

PHIDOT 

=  F  ( 1 0  ) 

THETAD 

=  F  (  1 1 ) 

PSIDOT 

=  F( 12) 

X 

U  =  INTGRL ( 6 . 0 , UDOT) 

V  =  INTGRL ( 0 . 0 , VDOT ) 

W  =  INTGRL (0.0, UDOT) 

P  =  INTGRL (0.0, PDOT ) 

Q  =  INTGRL (O.O.QDOT) 

R  =  INTGRL (O.O.RDOT) 

XPOS  =  INTGRL ( 0 . 0 , XDOTA ) 
YPOS  =  INTGRL ( 0 . 0 , YDOT ) 

ZPOS  =  INTGRL (0.0, ZDOT ) 

PHI  =  INTGRL(0.0,PHID0T) 
THETA  =  INTGRL ( 0 . 0 , THETAD) 
PSI  =  INTGRL (0 .0, PSI DOT) 
x 

ZNEW  =  -ZPOS 

PHI ANG  =  PHI/0.0174532925 
THEANG  =  THETA/0.0174532925 
PSIANG  =  PSI/0. 0174532925 
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*****  *  ****  *  *****  **** 


DYNAMIC 

RN=TIME/(FINTIM/10 . -DELT/10000. ) 

PN=TIME/(FINTIM/20 .-DELT/10000 . ) 

0=INT ( RN )+l 
PP=INT(PN)+1 
IFCPP.GE.20)  PP=20 
IF(0 . EQ. 11)  0=10 

ADDITIONALLY  THE  PLANES  SHOULD  BE  AT  EQUILIBRIUM  SO  THE 
VEHICLE  WILL  PROCEED  AT  THIS  NEW  DEPTH  WITHIN  SOME  TOLERANCE 

DSM=DX<0) 

DBSM=DX( 10+0) 

DBPM=DX( 10+0) 

IF(O.GE.IO)  DSM=0 • 

IF(O.GE.IO)  DBPM  =0.000 
IF(O.GE.IO)  DBSM  =0.000 
DRM=DXC20+PP) 

RPM*DX( 30+0) 

CONSTRAINTS  FOR  A  DIVE 

ORDERED  DEPTH  =  ORDDEP 

GW( 1 )  =  (ZPOSM-ORDDEP)X . 5 
GW(2)  =  (ORDDEP-ZPOSM)X . 5 
AUV'S  FINAL  STATE  MUST  BE  LEVEL  FLIGHT  AS  FOLLOWS 
GWC3)  =  THETAMX10. 

GW<4)  =  -THETAMX10. 

GW(5)=(YP0SM-Y0RD)/ .A 
GW(6)=(Y0RD-YP0SM)/.4 
GW<7)  =  -ZPOSM 

AVOIDING  THE  OBSTACLE 

IF  (DIST1 . LT.DSAVE1)  DSAVE1=DIST1 


IF  ( DISTV . LT . DSAVE1 )  DSAVEV=DISTV 

DIST1=S9RT(  (XP0SM-X0BS1  )*<  XPOSM-XOBSD  + .  . . 

(ZP0SM-Z0BS1 ) *( ZPOSM-ZOBSl ) ) 

DISTV =SQRT ( (XP0S-X0BS1 ) X( XP0S-X0B51 )+ . . . 

(ZP0S-Z0BS1 ) XCZP0S-Z0BS1 ) ) 

GW(8)  =  Cl.-DSAVEI) 

NDX=XP0SM/17 .425 
NDZ=ZP0SM/17 .425 
NDT=TIMEX6./17.425 
TERMINAL 

IF( INFO . EQ . 0 )  THEN 
PRINTX , DSAVE1 , DSAVEV 
9000  CONTINUE 
ELSE 
ENDIF 

IFC INFO . EQ . 0 )  CALL  ENDJOB 
CALL  RERUN 
x 

END 

STOP 
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